MCL: new parameters to set X and Y offset, used for particles initialization.

Updated robomon to use these parameters.

diff --git a/src/mcl/mcl.c b/src/mcl/mcl.c
index 1a8a6751489c42e2612636b2ce283684aea24967..79012e40cd4c3a4eb29a2e9a9f375956aaa053fa 100644 (file)
--- a/src/mcl/mcl.c
+++ b/src/mcl/mcl.c
@@ -300,8 +300,8 @@ void mcl_init (struct mcl_model *mcl)
                /* generate particles with noises. Starting weight is 1, as we 
                 * consider that any of the particles could be the right one.
                 */
-               parts[i].x = (rand()&0xffff)/65535.0*mcl->width;
-               parts[i].y = (rand()&0xffff)/65535.0*mcl->height;
+               parts[i].x = mcl->xoff + (rand()&0xffff)/65535.0*mcl->width;
+               parts[i].y = mcl->yoff + (rand()&0xffff)/65535.0*mcl->height;
                parts[i].angle = (rand() % 360)/360.0*(2.0*M_PI);
                parts[i].weight = 1;
 
@@ -313,8 +313,7 @@ void mcl_init (struct mcl_model *mcl)
 
                /*printf("x=%f, y=%f, a=%f, w=%f\n", 
                        parts[i].x, parts[i].y, 
-                       parts[i].angle, parts[i].weight);
-               */
+                       parts[i].angle, parts[i].weight);*/
        }                                
 }
 

diff --git a/src/mcl/mcl.h b/src/mcl/mcl.h
index 7270c0362fcfbe8c64fe20499dbd376b15f24bb3..b8fab2b8c8b0e16f7729c26c896d33a7b7246c3e 100644 (file)
--- a/src/mcl/mcl.h
+++ b/src/mcl/mcl.h
@@ -70,6 +70,8 @@ struct mcl_model {
        /* size of the playground (in meters) */
        double width;
        double height;
+       double xoff;            /* X offset */
+       double yoff;            /* Y offset */
        /* counters stuff */
        int cycle;              /* counter of updated cycles */
        int noisecycle;         /* counter of noise cycles */

diff --git a/src/robomon/src2/MclPainter.cpp b/src/robomon/src2/MclPainter.cpp
index 0657aa8864dd6aba8bbca9747adf2df2abd4b994..52d3483c46fc01e4ef4775ab2e08de73985e3683 100644 (file)
--- a/src/robomon/src2/MclPainter.cpp
+++ b/src/robomon/src2/MclPainter.cpp
@@ -38,9 +38,9 @@ void MclPainter::paint(QPainter *painter, QPaintEvent *event)
        for (int i=count; i<mcl->count; i++) {
                /* draw a circle */
                painter->translate(
-                       (qreal)(parts[i].x / mcl->width * width), 
-                       (qreal)((mcl->height-parts[i].y) 
-                               / mcl->height * height));
+                       (qreal)(parts[i].x / PLAYGROUND_WIDTH_M * width), 
+                       (qreal)((PLAYGROUND_HEIGHT_M-parts[i].y) 
+                               / PLAYGROUND_HEIGHT_M * height));
 
                col = (int)(parts[i].weight * 128 + 50);
                col = (col > 255) ? 255 : col;
@@ -49,22 +49,22 @@ void MclPainter::paint(QPainter *painter, QPaintEvent *event)
                painter->setBrush(QColor(col, 40, 40));
                painter->drawEllipse(0, 0, 10, 10);
                painter->translate(
-                       -(qreal)(parts[i].x / mcl->width * width), 
-                       -(qreal)((mcl->height-parts[i].y) 
-                               / mcl->height * height));
+                       -(qreal)(parts[i].x / PLAYGROUND_WIDTH_M * width), 
+                       -(qreal)((PLAYGROUND_HEIGHT_M-parts[i].y) 
+                               / PLAYGROUND_HEIGHT_M * height));
 
                /* draw a line representing the robot's head */
                painter->translate(
-                       (qreal)((parts[i].x / mcl->width * width)+5), 
-                       (qreal)(((mcl->height-parts[i].y) 
-                               / mcl->height * height)+5));
+                       (qreal)((parts[i].x / PLAYGROUND_WIDTH_M * width)+5), 
+                       (qreal)(((PLAYGROUND_HEIGHT_M-parts[i].y) 
+                               / PLAYGROUND_HEIGHT_M * height)+5));
                painter->rotate(RAD2DEG(-parts[i].angle));
                painter->drawLine(0, 0, 20, 0);
                painter->rotate(RAD2DEG(parts[i].angle));
                painter->translate(
-                       -(qreal)((parts[i].x / mcl->width * width)+5), 
-                       -(qreal)(((mcl->height-parts[i].y) 
-                               / mcl->height * height)+5));
+                       -(qreal)((parts[i].x / PLAYGROUND_WIDTH_M * width)+5), 
+                       -(qreal)(((PLAYGROUND_HEIGHT_M-parts[i].y) 
+                               / PLAYGROUND_HEIGHT_M * height)+5));
        }
 }
 

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 8b98fe8b0818fdc7106c459aa0b25e2d23dc7428..a5c7e6e005c2d77cba434bb58f0e80d55bea343a 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -222,6 +222,8 @@ void SimMcl::createMclGroupBox()
 
        mclWidthLineEdit = new QLineEdit();
        mclHeightLineEdit = new QLineEdit();
+       mclXOffLineEdit = new QLineEdit();
+       mclYOffLineEdit = new QLineEdit();
        mclGenDnoiseLineEdit = new QLineEdit();
        mclGenAnoiseLineEdit = new QLineEdit();
        mclMovDnoiseLineEdit = new QLineEdit();
@@ -237,19 +239,21 @@ void SimMcl::createMclGroupBox()
 
        layout->addWidget(MiscGui::createLabel("width"), 0, 0);
        layout->addWidget(MiscGui::createLabel("height"), 1, 0);
-       layout->addWidget(MiscGui::createLabel("gen_dnoise"), 2, 0);
-       layout->addWidget(MiscGui::createLabel("gen_anoise"), 3, 0);
-       layout->addWidget(MiscGui::createLabel("mov_dnoise"), 4, 0);
-       layout->addWidget(MiscGui::createLabel("mov_anoise"), 5, 0);
-       layout->addWidget(MiscGui::createLabel("beacon_cnt"), 6, 0);
+       layout->addWidget(MiscGui::createLabel("x offset"), 2, 0);
+       layout->addWidget(MiscGui::createLabel("y offset"), 3, 0);
+       layout->addWidget(MiscGui::createLabel("gen_dnoise"), 4, 0);
+       layout->addWidget(MiscGui::createLabel("gen_anoise"), 5, 0);
+       layout->addWidget(MiscGui::createLabel("mov_dnoise"), 6, 0);
+       layout->addWidget(MiscGui::createLabel("mov_anoise"), 7, 0);
 
        layout->addWidget(mclWidthLineEdit, 0, 1);
        layout->addWidget(mclHeightLineEdit, 1, 1);
-       layout->addWidget(mclGenDnoiseLineEdit, 2, 1);
-       layout->addWidget(mclGenAnoiseLineEdit, 3, 1);
-       layout->addWidget(mclMovDnoiseLineEdit, 4, 1);
-       layout->addWidget(mclMovAnoiseLineEdit, 5, 1);
-       layout->addWidget(mclBeaconCount, 6, 1);
+       layout->addWidget(mclXOffLineEdit, 2, 1);
+       layout->addWidget(mclYOffLineEdit, 3, 1);
+       layout->addWidget(mclGenDnoiseLineEdit, 4, 1);
+       layout->addWidget(mclGenAnoiseLineEdit, 5, 1);
+       layout->addWidget(mclMovDnoiseLineEdit, 6, 1);
+       layout->addWidget(mclMovAnoiseLineEdit, 7, 1);
 
        layout->addWidget(MiscGui::createLabel("count"), 0, 2);
        layout->addWidget(MiscGui::createLabel("w_min"), 1, 2);
@@ -258,6 +262,7 @@ void SimMcl::createMclGroupBox()
        layout->addWidget(MiscGui::createLabel("aeval sigma"), 4, 2);
        layout->addWidget(MiscGui::createLabel("maxavdist"), 5, 2);
        layout->addWidget(MiscGui::createLabel("maxnoisecycle"), 6, 2);
+       layout->addWidget(MiscGui::createLabel("beacon_cnt"), 7, 2);
 
        layout->addWidget(mclCountLineEdit, 0, 3);
        layout->addWidget(mclWMinLineEdit, 1, 3);
@@ -266,12 +271,15 @@ void SimMcl::createMclGroupBox()
        layout->addWidget(mclAEvalSigmaLineEdit, 4, 3);
        layout->addWidget(mclMaxavdistLineEdit, 5, 3);
        layout->addWidget(mclMaxnoisecycleLineEdit, 6, 3);
+       layout->addWidget(mclBeaconCount, 7, 3);
 
        mclWidthLineEdit->setMinimumWidth(50);
        mclCountLineEdit->setMinimumWidth(50);
 
        mclWidthLineEdit->setText("3.000");
        mclHeightLineEdit->setText("2.100");
+       mclXOffLineEdit->setText("0.000");
+       mclYOffLineEdit->setText("0.000");
        mclGenDnoiseLineEdit->setText("0.99");
        mclGenAnoiseLineEdit->setText("360");
        mclMovDnoiseLineEdit->setText("0.01");
@@ -508,7 +516,8 @@ void SimMcl::initPaintObjects()
                robots[i].mclPart = (struct mcl_particle *)&parts[i];
                robots[i].widgetSize = QSize((int)playgroundScene->width(), 
                                        (int)playgroundScene->height());
-               robots[i].playgroundSize = QSizeF(mcl.width, mcl.height);
+               robots[i].playgroundSize = 
+                       QSizeF(PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M);
                robots[i].init();
                robots[i].setParent(this);
                playgroundScene->addItem(&robots[i]);
@@ -686,8 +695,8 @@ void SimMcl::useRedBeacons(bool state)
 void SimMcl::generateMeasurement()
 {
        mcl_thetas theta;
-       measPos.x = (double)((qrand()&0xffff)/65535.0*(double)mcl.width);
-       measPos.y = (double)((qrand()&0xffff)/65535.0*(double)mcl.height);
+       measPos.x = (double)(mcl.xoff+(qrand()&0xffff)/65535.0*(double)mcl.width);
+       measPos.y = (double)(mcl.yoff+(qrand()&0xffff)/65535.0*(double)mcl.height);
        measPos.angle = DEG2RAD((double)((qrand()&0xffff)/65535.0*360));
        /* convert generated position to angles between reflectors */
        mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
@@ -1013,6 +1022,8 @@ void SimMcl::countEstimatedAnglesFrequency()
 void SimMcl::initMcl()
 {
        /* MCL initialization */
+       mcl.xoff = mclXOffLineEdit->text().toFloat();/* in meters */
+       mcl.yoff = mclYOffLineEdit->text().toFloat();/* in meters */
        mcl.width = mclWidthLineEdit->text().toFloat();/* in meters */
        mcl.height = mclHeightLineEdit->text().toFloat();/* in meters */
        /* the noises */

diff --git a/src/robomon/src2/SimMcl.h b/src/robomon/src2/SimMcl.h
index cdf75b3d7d1f88f8a024d75059217bf158204f71..6059d84b5485b81ce153bf131e5974487185838f 100644 (file)
--- a/src/robomon/src2/SimMcl.h
+++ b/src/robomon/src2/SimMcl.h
@@ -144,6 +144,8 @@ private:
        /* mcl gui */
        QLineEdit *mclWidthLineEdit;
        QLineEdit *mclHeightLineEdit;
+       QLineEdit *mclXOffLineEdit;
+       QLineEdit *mclYOffLineEdit;
        QLineEdit *mclGenDnoiseLineEdit;
        QLineEdit *mclGenAnoiseLineEdit;
        QLineEdit *mclMovDnoiseLineEdit;