1 files changed, 2 insertions, 43 deletions
diff --git a/src/main.cpp b/src/main.cpp
index e7e08a5..bd31da9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -170,51 +170,10 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
         Vision viz;
         viz.open(msg.image_names[0],cam);
 
-        strcat(msg.image_names[0],".txt");
-        FILE *fin;
-        if ((fin=fopen(msg.image_names[0], "r"))==NULL) {
-            err_sys("fopen: %s", msg.image_names[0]);
-        }
-        int id,sx,sy,rx,ry;
-        int Nmatched;
-        while ((Nmatched=fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry))!=EOF) {
-            measurement_t m;
-            m.id = id;
-            // Points in the image frame
-            Vector3d xi, xir;
-            xi << sx, sy, 1.;
-            m.source = cam.img2body(xi);
-            if (Nmatched==5) {
-                xir << rx, ry, 1.;
-                m.reflection = cam.img2body(xir);
-                m.z_type = REFLECTION;
-            } else {
-                m.z_type = MONO;
-            }
-            // Points in the camera frame
-            z.push_back(m);
-        }
-        if (fclose(fin)==EOF) {
-            err_sys("fclose");
-        }
-        // The measured values
-        //viz.drawMeasurements(z,cam, cv::Scalar(0,255,0), cv::Scalar(0,180,0),false);
-        // The predicted values
-        std::vector<measurement_t> ykk1;
-        std::vector<measurement_t> newFeatures;
-        viz.acquireFeatures(cam, newFeatures);
-        viz.drawMeasurements(newFeatures, cam, cv::Scalar(0,0,255), cv::Scalar(0,0,180),false);
-#if STATESIZE==13
-        //mu.featuresAsMeasurements(ykk1);
-#else
-        //mu.featuresAsMeasurements(ykk1,q);
-#endif
-        //viz.drawMeasurements(ykk1, cam, cv::Scalar(255,0,0), cv::Scalar(180,0,0),true);
-        viz.show();
 #if STATESIZE==13
-        //mu.handle_measurements(z);
+        mu.doMeasurements(z,viz,cam);
 #else
-        mu.handle_measurements(z,q);
+        mu.doMeasurements(z,viz,q,cam);
 #endif
     }