Allow for the usage of REFLECTION and MONO features.

Feature methods were updated to accomodate both types of features.

1 files changed, 12 insertions, 7 deletions

diff --git a/src/main.cpp b/src/main.cpp
index eb1cdc1..fc08e7a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -86,17 +86,22 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
             err_sys("fopen: %s", msg.image_names[0]);
         }
         int id,sx,sy,rx,ry;
-        while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) {
+        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.;
-            xir << rx, ry, 1.;
-            // Points in the camera frame
             m.source = cam.img2body(xi);
-            m.reflection = cam.img2body(xir);
-            m.z_type = REFLECTION;
+            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) {
@@ -184,7 +189,7 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q)
     } else {
         /*
         i+=1;
-        if (i%18==0) {
+        if (i%12==0) {
             UTM utm_water;
             utm_water.northing = msg.utm.northing;
             utm_water.easting = msg.utm.easting;
@@ -347,7 +352,7 @@ main(int argc, char **argv)
     ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
 #else      /* -----  not ROS_PUBLISH  ----- */
     // Print unicsv header
-    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
+    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #endif     /* -----  not ROS_PUBLISH  ----- */
     // Read sensors from file
     char line[MAXLINE];