MCL: Put thetas away from mcl_particle

MPC5200 has only 16 kB data cache so we have to keep working set as
small as possible to limit slow loads/stores from/to the main memory.

diff --git a/src/mcl/mcl.c b/src/mcl/mcl.c
index c6eadb71f9250c2e2457643f5c0f16010ba6b2a8..47cc5bdf02b3dab5c42065dbd047779623a7ccd6 100644 (file)
--- a/src/mcl/mcl.c
+++ b/src/mcl/mcl.c
@@ -118,6 +118,7 @@ void mcl_update2(struct mcl_model *mcl, void *data)
 {
        struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
        struct mcl_angles *angles = (struct mcl_angles*)data;
+       mcl_thetas theta;
        double p[mcl->beacon_cnt], p1;
        int i, it, im;
 
@@ -145,7 +146,7 @@ void mcl_update2(struct mcl_model *mcl, void *data)
 
        /* evaluate the weights of each particle */
        for (i=0; i<mcl->count; i++) {
-               mcl_pos2ang(&parts[i], mcl->beacon_cnt, mcl->beacon_color);
+               mcl_pos2ang(&parts[i], theta, mcl->beacon_cnt, mcl->beacon_color);
                for (it=0; it<mcl->beacon_cnt; it++) {
                        p[it] = 1.0;
                        for (im=0; im<angles->count; im++) { 
@@ -158,7 +159,7 @@ void mcl_update2(struct mcl_model *mcl, void *data)
                                   density */
                                /* printf("part_ang=%g measured_ang=%g\n", 
                                                parts[i].theta[it], angles->val[im]); */
-                               p1 = evaluate_angles(parts[i].theta[it],
+                               p1 = evaluate_angles(theta[it],
                                                angles->val[im], mcl->aeval_sigma);
                                /* printf("p1 = %f\n", p1); */
                                /* FIXME: p[it] is initialized to 1
@@ -354,16 +355,20 @@ inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl)
        return (((struct mcl_particle *)mcl->parts)[mcl->count-1]);
 }
 
+
+
 /**
  * Convert the position to angles between robot's head and reflectors 
  * (angles sense is counter-clockwise)
  *
  * @param  part                MCL particle
+ * @param  theta       Returned angles to reflectors
  * @param  color       beacon color
  * @param  cnt         beacon count
  */
-void mcl_pos2ang(struct mcl_particle *part, 
-                       unsigned char cnt, unsigned char color)
+void mcl_pos2ang(const struct mcl_particle *part,
+                mcl_thetas theta,
+                unsigned char cnt, unsigned char color)
 {
        double head;
        int i;
@@ -380,29 +385,26 @@ void mcl_pos2ang(struct mcl_particle *part,
 
        /* angles between 0 and reflectors (with center at robot position) */
        if (color == BEACON_RED) {
-               part->theta[0] = atan2(BEACON_RED1_Y-part->y, BEACON_RED1_X-part->x);
-               part->theta[1] = atan2(BEACON_RED2_Y-part->y, BEACON_RED2_X-part->x);
-               part->theta[2] = atan2(BEACON_RED3_Y-part->y, BEACON_RED3_X-part->x);
-               part->theta[3] = atan2(BEACON_RED4_Y-part->y, BEACON_RED4_X-part->x);
+               theta[0] = atan2(BEACON_RED1_Y-part->y, BEACON_RED1_X-part->x);
+               theta[1] = atan2(BEACON_RED2_Y-part->y, BEACON_RED2_X-part->x);
+               theta[2] = atan2(BEACON_RED3_Y-part->y, BEACON_RED3_X-part->x);
+               theta[3] = atan2(BEACON_RED4_Y-part->y, BEACON_RED4_X-part->x);
        } else {
-               part->theta[0] = atan2(BEACON_BLUE1_Y-part->y, BEACON_BLUE1_X-part->x);
-               part->theta[1] = atan2(BEACON_BLUE2_Y-part->y, BEACON_BLUE2_X-part->x);
-               part->theta[2] = atan2(BEACON_BLUE3_Y-part->y, BEACON_BLUE3_X-part->x);
-               part->theta[3] = atan2(BEACON_BLUE4_Y-part->y, BEACON_BLUE4_X-part->x);
+               theta[0] = atan2(BEACON_BLUE1_Y-part->y, BEACON_BLUE1_X-part->x);
+               theta[1] = atan2(BEACON_BLUE2_Y-part->y, BEACON_BLUE2_X-part->x);
+               theta[2] = atan2(BEACON_BLUE3_Y-part->y, BEACON_BLUE3_X-part->x);
+               theta[3] = atan2(BEACON_BLUE4_Y-part->y, BEACON_BLUE4_X-part->x);
        }
 
-       /* normalize angles */
-       for (i=0; i<cnt; i++)
-               part->theta[i] = (part->theta[i] < 0) ? 
-                       2*M_PI + part->theta[i] : part->theta[i];
-
-       /* offset to the head angle */
-       for (i=0; i<cnt; i++)
-               part->theta[i] = (part->theta[i]-head < 0) ? 
-                       2*M_PI+part->theta[i]-head : part->theta[i]-head;
+       for (i=0; i<cnt; i++) {
+               /* normalize angles */
+               if (theta[i] < 0) theta[i] = 2*M_PI + theta[i];
+               theta[i] = (theta[i]-head < 0) ? 
+                       2*M_PI+theta[i]-head : theta[i]-head;
+       }
 
        /* sort and store results */
-       qsort(part->theta, cnt, sizeof(double), cmpd);
+       qsort(theta, cnt, sizeof(double), cmpd);
 }
 
 /**

diff --git a/src/mcl/mcl.h b/src/mcl/mcl.h
index 6313e244eaf750d981f9f62c561bc720cb5b63ae..7270c0362fcfbe8c64fe20499dbd376b15f24bb3 100644 (file)
--- a/src/mcl/mcl.h
+++ b/src/mcl/mcl.h
@@ -26,7 +26,6 @@ struct mcl_particle {
        double y;
        double angle;
        double weight; 
-       double theta[BEACON_CNT];
 };
 
 /* Estimated position */
@@ -36,6 +35,8 @@ struct mcl_position {
        double angle;
 };
 
+typedef double mcl_thetas[BEACON_CNT];
+
 /* Measured angles */
 struct mcl_angles {
        int count;
@@ -102,8 +103,9 @@ void mcl_partsort(struct mcl_model *mcl);
 void mcl_resample(struct mcl_model *mcl);
 void mcl_init (struct mcl_model *mcl);
 inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl);
-void mcl_pos2ang(struct mcl_particle *part, 
-                       unsigned char cnt, unsigned char color);
+void mcl_pos2ang(const struct mcl_particle *part,
+                mcl_thetas theta,
+                unsigned char cnt, unsigned char color);
 #ifdef __cplusplus
 }
 #endif 

diff --git a/src/mcl/test/testmcl_performance.c b/src/mcl/test/testmcl_performance.c
index 807d69349ab9356217298a5e55b734343a09b09a..22118f18cf87ae20a9ffe78d6582891fb2e1807b 100644 (file)
--- a/src/mcl/test/testmcl_performance.c
+++ b/src/mcl/test/testmcl_performance.c
@@ -30,6 +30,7 @@ struct mcl_angles meas_angles;
 struct mcl_particle act_pos;
 struct mcl_particle est_pos;
 struct mcl_particle meas_pos;
+mcl_thetas theta;
 struct mcl_particle *parts;
 
 /**
@@ -122,10 +123,10 @@ int main(int argc, char *argv[])
        meas_pos.x = 1.0;
        meas_pos.y = 1.0;
        meas_pos.angle = DEG2RAD(180);
-       mcl_pos2ang(&meas_pos, mcl.beacon_cnt, mcl.beacon_cnt);
+       mcl_pos2ang(&meas_pos, theta, mcl.beacon_cnt, mcl.beacon_cnt);
        meas_angles.count = mcl.beacon_cnt;
        for(i=0; i<mcl.beacon_cnt; i++)
-               meas_angles.val[i] = meas_pos.theta[i];
+               meas_angles.val[i] = theta[i];
 
 /*     printf("meas: x=%g y=%g angle=%g w=%g th1=%g th2=%g th3=%g\n",
                meas_pos.x, meas_pos.y, meas_pos.angle,

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 26e7c420318317cd99e299b49d81adfa42cf8335..35448e01dd7590434e1690fb3d0cb85bec61873d 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -683,14 +683,15 @@ 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.angle = DEG2RAD((double)((qrand()&0xffff)/65535.0*360));
        /* convert generated position to angles between reflectors */
-       mcl_pos2ang(&measPos, mcl.beacon_cnt, mcl.beacon_color);
+       mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
        measuredAngles.count = mcl.beacon_cnt;
        for (int i=0; i<mcl.beacon_cnt; i++)
-               measuredAngles.val[i] = measPos.theta[i];
+               measuredAngles.val[i] = theta[i];
 
        QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
                        .arg(measPos.x, 0, 'f', 3)
@@ -700,9 +701,9 @@ void SimMcl::generateMeasurement()
        dbg.append(QString("  ["));
        for (int i=0; i<mcl.beacon_cnt-1; i++)
                dbg.append(QString("%1  ")
-               .arg(RAD2DEG(measPos.theta[i]), 0, 'f', 2));
+               .arg(RAD2DEG(theta[i]), 0, 'f', 2));
        dbg.append(QString("%1")
-               .arg(RAD2DEG(measPos.theta[mcl.beacon_cnt-1]), 0, 'f', 2));
+               .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
        dbg.append(QString("]"));
 
        WDBG(dbg);
@@ -876,11 +877,6 @@ void SimMcl::initPln()
 
 void SimMcl::updatePln()
 {
-       struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
-
-       for (int i=0; i<mcl.count; i++) {
-               mcl_pos2ang(&parts[i], mcl.beacon_cnt, mcl.beacon_color);
-       }
 }
 
 void SimMcl::initPlnData()
@@ -925,6 +921,7 @@ void SimMcl::countEstimatedAnglesFrequency()
 {
        struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
        double a;
+       mcl_thetas theta;
        int _angles[ANGLE_FREQ_COUNT];
 
        if (resetHistogramsPeriodicallyCheckBox->isChecked())
@@ -934,9 +931,10 @@ void SimMcl::countEstimatedAnglesFrequency()
                _angles[i] = 0;
 
        for (int i=0; i<displayCountSlider->value(); i++) {
+               mcl_pos2ang(&parts[i], theta, mcl.beacon_cnt, mcl.beacon_color);
                for (int e=0; e<mcl.beacon_cnt; e++) {
                        /* normalize angles */
-                       a = fabs(fmod(RAD2DEG(parts[i].theta[e]), 360));
+                       a = fabs(fmod(RAD2DEG(theta[e]), 360));
                        _angles[(int)a]++;
                }
        }
@@ -1015,6 +1013,7 @@ void SimMcl::initMcl()
 void SimMcl::moveParts(double dx, double dy, double dangle)
 {
        int i=0;
+       mcl_thetas theta;
 
        mcl.cycle++;
        /*WDBG(QString("mcl cycle = %1").arg(mcl.cycle));*/
@@ -1031,10 +1030,10 @@ void SimMcl::moveParts(double dx, double dy, double dangle)
                measuredPosition[1] = measPos.y;
                measuredPosition[2] = measPos.angle;
 
-               mcl_pos2ang(&measPos, mcl.beacon_cnt, mcl.beacon_color);
+               mcl_pos2ang(&measPos, theta, mcl.beacon_cnt, mcl.beacon_color);
                measuredAngles.count = mcl.beacon_cnt;
                for (int i=0; i<mcl.beacon_cnt; i++)
-                       measuredAngles.val[i] = measPos.theta[i];
+                       measuredAngles.val[i] = theta[i];
                QString dbg = QString("(test) gen. meas.: x=%1 y=%2 head=%3 ")
                                .arg(measPos.x, 0, 'f', 3)
                                .arg(measPos.y, 0, 'f', 3)
@@ -1043,9 +1042,9 @@ void SimMcl::moveParts(double dx, double dy, double dangle)
                dbg.append(QString("  ["));
                for (int i=0; i<mcl.beacon_cnt-1; i++)
                        dbg.append(QString("%1  ")
-                       .arg(RAD2DEG(measPos.theta[i]), 0, 'f', 2));
+                       .arg(RAD2DEG(theta[i]), 0, 'f', 2));
                dbg.append(QString("%1")
-                       .arg(RAD2DEG(measPos.theta[mcl.beacon_cnt-1]), 0, 'f', 2));
+                       .arg(RAD2DEG(theta[mcl.beacon_cnt-1]), 0, 'f', 2));
                dbg.append(QString("]"));
 
                WDBG(dbg);

diff --git a/src/robomon/src2/SmallRobot.cpp b/src/robomon/src2/SmallRobot.cpp
index 69db4120f14a24c1ef1302ca83eb060368cf826e..663a65bd18beda95de4b4dfd42ddf162eb7b23d5 100644 (file)
--- a/src/robomon/src2/SmallRobot.cpp
+++ b/src/robomon/src2/SmallRobot.cpp
@@ -70,22 +70,27 @@ void SmallRobot::move()
 
 void SmallRobot::updateRobot()
 {
+       mcl_thetas theta;
        resetTransform();
        rotate(RAD2DEG(-mclPart->angle));
        setPos((qreal)(mclPart->x / playgroundSize.width() * widgetSize.width()), 
                (qreal)((playgroundSize.height() - mclPart->y)
                        / playgroundSize.height() * widgetSize.height()));
 
+       //FIXME: I do not know how calculate thetas here. Michal
+       //mcl_pos2ang(&mclPart, theta, mcl.beacon_cnt, mcl.beacon_color);
        setToolTip(QString("X=%1 Y=%2\nangle=%3\nweight=%4"
-                       "\nth1=%5\nth2=%6\nth3=%7\nth4=%8")
+                       // "\nth1=%5\nth2=%6\nth3=%7\nth4=%8"
+                          )
                        .arg(mclPart->x, 0, 'f', 3)
                        .arg(mclPart->y, 0, 'f', 3)
                        .arg(RAD2DEG(mclPart->angle), 0, 'f', 3)
                        .arg(mclPart->weight, 0, 'f', 5)
-                       .arg(RAD2DEG(mclPart->theta[0]), 0, 'f', 3)
-                       .arg(RAD2DEG(mclPart->theta[1]), 0, 'f', 3)
-                       .arg(RAD2DEG(mclPart->theta[2]), 0, 'f', 3)
-                       .arg(RAD2DEG(mclPart->theta[3]), 0, 'f', 3));
+                       // .arg(RAD2DEG(mclPart->theta[0]), 0, 'f', 3)
+//                     .arg(RAD2DEG(mclPart->theta[1]), 0, 'f', 3)
+//                     .arg(RAD2DEG(mclPart->theta[2]), 0, 'f', 3)
+//                     .arg(RAD2DEG(mclPart->theta[3]), 0, 'f', 3)
+               );
 
        int col = (int)(mclPart->weight * 128 + 50);
        col = (col > 255) ? 255 : col;