robomon2: sigma of the evaluation function descreases after the update, seems to...

diff --git a/src/laser-nav/mcl/mcl.c b/src/laser-nav/mcl/mcl.c
index bea5072e1beaebabc41bef0fd94a5d203292896e..ca8bf29b103c05cd8b08be0eeaae8b0ae4e7fbac 100644 (file)
--- a/src/laser-nav/mcl/mcl.c
+++ b/src/laser-nav/mcl/mcl.c
@@ -109,6 +109,13 @@ void mcl_update2(struct mcl_model *mcl, void *data)
        int i, it, im;
        double *angles = (double *)data;
 
+       /* decrease the sigma */
+       if (mcl->aeval_sigma > 0.005) {
+               mcl->aeval_sigma *= (double)0.9;
+       } else {
+               mcl->aeval_sigma = 0.1;
+       }
+
        /* evaluate the weights of each particle */
        for (i=0; i<mcl->count; i++) {
                for (it=0; it<3; it++) {
@@ -117,7 +124,7 @@ void mcl_update2(struct mcl_model *mcl, void *data)
                                /* evaluate weights with gaussian probability 
                                   density */
                                p1 = evaluate_angles(parts[i].theta[it],
-                                       angles[im], DEG2RAD(2.0), 0.05);
+                                       angles[im], DEG2RAD(2.0), mcl->aeval_sigma);
                                p[it] = (p1 > p[it]) ? p1 : p[it];
                        }
                }

diff --git a/src/laser-nav/mcl/mcl.h b/src/laser-nav/mcl/mcl.h
index c3b674e18d8df2700152eac21e3fd3a87748cb53..d16d4de0d16876c18828e872c92029f10d686aac 100644 (file)
--- a/src/laser-nav/mcl/mcl.h
+++ b/src/laser-nav/mcl/mcl.h
@@ -49,6 +49,7 @@ struct mcl_model {
        double w_min;
        double w_max;
        double eval_sigma;
+       double aeval_sigma;
        /* size of the playground */
        int width;
        int height;

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 609527d966cd2420775381f274d0cb60c3d59d0c..cb826fc3e083a049a0abe8af813a276b7d8da5b9 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -209,6 +209,7 @@ void SimMcl::createMclGroupBox()
        mclWMinLineEdit = new QLineEdit();
        mclWMaxLineEdit = new QLineEdit();
        mclEvalSigmaLineEdit = new QLineEdit();
+       mclAEvalSigmaLineEdit = new QLineEdit();
        mclMaxavdistLineEdit = new QLineEdit();
        mclMaxnoisecycleLineEdit = new QLineEdit();
        mclCountLineEdit = new QLineEdit();
@@ -231,15 +232,17 @@ void SimMcl::createMclGroupBox()
        layout->addWidget(MiscGui::createLabel("w_min"), 1, 2);
        layout->addWidget(MiscGui::createLabel("w_max"), 2, 2);
        layout->addWidget(MiscGui::createLabel("eval sigma"), 3, 2);
-       layout->addWidget(MiscGui::createLabel("maxavdist"), 4, 2);
-       layout->addWidget(MiscGui::createLabel("maxnoisecycle"), 5, 2);
+       layout->addWidget(MiscGui::createLabel("aeval sigma"), 4, 2);
+       layout->addWidget(MiscGui::createLabel("maxavdist"), 5, 2);
+       layout->addWidget(MiscGui::createLabel("maxnoisecycle"), 6, 2);
 
        layout->addWidget(mclCountLineEdit, 0, 3);
        layout->addWidget(mclWMinLineEdit, 1, 3);
        layout->addWidget(mclWMaxLineEdit, 2, 3);
        layout->addWidget(mclEvalSigmaLineEdit, 3, 3);
-       layout->addWidget(mclMaxavdistLineEdit, 4, 3);
-       layout->addWidget(mclMaxnoisecycleLineEdit, 5, 3);
+       layout->addWidget(mclAEvalSigmaLineEdit, 4, 3);
+       layout->addWidget(mclMaxavdistLineEdit, 5, 3);
+       layout->addWidget(mclMaxnoisecycleLineEdit, 6, 3);
 
        mclWidthLineEdit->setText("3000");
        mclHeightLineEdit->setText("2100");
@@ -252,6 +255,7 @@ void SimMcl::createMclGroupBox()
        mclWMinLineEdit->setText("0.25");
        mclWMaxLineEdit->setText("2.0");
        mclEvalSigmaLineEdit->setText("160");
+       mclAEvalSigmaLineEdit->setText("0.90");
        mclMaxavdistLineEdit->setText("150");
        mclMaxnoisecycleLineEdit->setText("10");
 
@@ -711,15 +715,6 @@ void SimMcl::laserReceived()
 
        WDBG("ORTE received: laser");
 
-//     /* FIXME: just to test, remove it */
-//     orteData.orteLaserData.cnt = 5;
-//     orteData.orteLaserData.period = 54619;
-//     orteData.orteLaserData.measures0 = 1714;
-//     orteData.orteLaserData.measures1 = 12312;
-//     orteData.orteLaserData.measures2 = 34549;
-//     orteData.orteLaserData.measures3 = 41231;
-//     orteData.orteLaserData.measures4 = 49670;
-
        cnt = orteData.orteLaserData.cnt;
        times[0] = orteData.orteLaserData.period;
        times[1] = orteData.orteLaserData.measures0;
@@ -733,6 +728,12 @@ void SimMcl::laserReceived()
        times[9] = orteData.orteLaserData.measures8;
        times[10] = orteData.orteLaserData.measures9;
 
+       /* avoid segfault when receiving some error */
+       if (cnt > (LAS_CNT-1)) {
+               WDBG(QString("Detected a bad number of received laser data: %1")
+                       .arg(cnt));
+       }
+
        measuredAngles[0] = (double)cnt;
        for (int i=1; i<cnt+1; i++) {
                measuredAngles[i] = (double)TIME2ANGLE(times[0],times[i]);
@@ -873,6 +874,7 @@ void SimMcl::initMcl()
        mcl.w_min = mclWMinLineEdit->text().toFloat();
        mcl.w_max = mclWMaxLineEdit->text().toFloat();
        mcl.eval_sigma = mclEvalSigmaLineEdit->text().toFloat();
+       mcl.aeval_sigma = mclAEvalSigmaLineEdit->text().toFloat();
        mcl.maxavdist = mclMaxavdistLineEdit->text().toFloat();
        /* bad cycles before reseting */
        mcl.maxnoisecycle = mclMaxnoisecycleLineEdit->text().toInt();
@@ -914,9 +916,7 @@ void SimMcl::initMcl()
 void SimMcl::moveParts(double dx, double dy, double dangle)
 {
        mcl.cycle++;
-//     WDBG(QString("mcl cycle = %1").arg(mcl.cycle));
-       /* move particles */
-       mcl.move(&mcl, dx, dy, dangle);
+       /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
 
        /* FIXME */
        /* normally, the [x,y,angle] are values measured by sensors. In this 
@@ -925,8 +925,9 @@ void SimMcl::moveParts(double dx, double dy, double dangle)
        measuredPosition[1] = mcl.height/2 + qrand() % 20;
        measuredPosition[2] = DEG2RAD(10);
 
+       /* MCL */
+       mcl.move(&mcl, dx, dy, dangle);
        mcl.update(&mcl, mcl.data);
-
        mcl.normalize(&mcl);
        mcl.sort(&mcl);
        mcl.resample(&mcl);

diff --git a/src/robomon/src2/SimMcl.h b/src/robomon/src2/SimMcl.h
index f2338792b63df3245c624f1b6f7b05e928e46ed5..e442a22ee1eb55ed34a4fdc46114aa2893b6018b 100644 (file)
--- a/src/robomon/src2/SimMcl.h
+++ b/src/robomon/src2/SimMcl.h
@@ -134,6 +134,7 @@ private:
        QLineEdit *mclWMinLineEdit;
        QLineEdit *mclWMaxLineEdit;
        QLineEdit *mclEvalSigmaLineEdit;
+       QLineEdit *mclAEvalSigmaLineEdit;
        QLineEdit *mclMaxavdistLineEdit;
        QLineEdit *mclMaxnoisecycleLineEdit;
        QLineEdit *mclCountLineEdit;