Add feature handling to state

1 files changed, 14 insertions, 3 deletions

diff --git a/src/main.cpp b/src/main.cpp
index af602c5..390a57e 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -43,8 +43,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 }
 
 void
-imgCallback(message &msg)
+imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
 {
+    std::vector<measurement_t> z;
     int id,sx,sy,rx,ry;
     strcat(msg.image_names[0],".txt");
     FILE *fin;
@@ -52,11 +53,21 @@ imgCallback(message &msg)
         err_sys("fopen: %s", msg.image_names[0]);
     }
     while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) {
-        printf("id: %d source: (%d, %d) reflection: (%d, %d)\n", id, sx, sy, rx, ry);
+        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);
+        z.push_back(m);
     }
     if (fclose(fin)==EOF) {
         err_sys("fclose");
     }
+    mu.feature_update(z,q);
 
     return;
 }
@@ -261,7 +272,7 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case IMG:	
-            imgCallback(msg);
+            imgCallback(msg, mu, cam, qbw);
             break;
 
         case INSPVAS: