From d020ec03598b90fb8d78194554471b1cfb4794dd Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 22 Mar 2017 12:23:24 -0500
Subject: Read in image measurements from file

---
 src/main.cpp | 21 +++++++++++++++++++--
 src/main.h   |  3 ++-
 2 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index cde65f5..af602c5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -43,8 +43,21 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 }
 
 void
-imgCallback(const message &msg)
+imgCallback(message &msg)
 {
+    int id,sx,sy,rx,ry;
+    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]);
+    }
+    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);
+    }
+    if (fclose(fin)==EOF) {
+        err_sys("fclose");
+    }
+
     return;
 }
 
@@ -100,7 +113,6 @@ utmCallback(const message &msg, State &mu)
     }
     return;
 }
-#endif     /* -----  not USE_ROS  ----- */
 
 int 
 parseLine(char *line, message *data)
@@ -192,6 +204,7 @@ parseLine(char *line, message *data)
         exit(1);
     }
 }
+#endif     /* -----  not USE_ROS  ----- */
 
 timestamp 
 update_dt(const timestamp t, timestamp *t_old)
@@ -215,6 +228,10 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
+    if (argc!=2) {
+        fprintf(stderr, "Usage: %s camera\n", argv[0]);
+        exit(1);
+    }
     State mu;
     Quaterniond qbw;
     // bias in FRD coordinates
diff --git a/src/main.h b/src/main.h
index b6d181e..174ab0e 100644
--- a/src/main.h
+++ b/src/main.h
@@ -10,6 +10,7 @@
 
 #include "body.h"
 #include "camera.h"
+#include "ourerr.hpp"
 #include "state.h"
 #include "types.h"
 
@@ -23,7 +24,7 @@ int parseLine(char *line, message *msg);
 timestamp update_dt(const timestamp t, timestamp *t_old);
 
 void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaterniond &q);
-void imgCallback(const message &msg);
+void imgCallback(message &msg);
 void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt);
 void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q);
 void utmCallback(const message &msg, State &mu);
-- 
cgit v1.1