Set height above water.

1 files changed, 45 insertions, 30 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 390a57e..bcf6b37 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -45,29 +45,32 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 void
 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;
-    if ((fin=fopen(msg.image_names[0], "r"))==NULL) {
-        err_sys("fopen: %s", msg.image_names[0]);
+    if (seenutm && seenpva && seencov) {
+        std::vector<measurement_t> z;
+        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) {
+            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;
+            z.push_back(m);
+        }
+        if (fclose(fin)==EOF) {
+            err_sys("fclose");
+        }
+        mu.feature_update(z,q);
     }
-    while (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);
-        z.push_back(m);
-    }
-    if (fclose(fin)==EOF) {
-        err_sys("fclose");
-    }
-    mu.feature_update(z,q);
 
     return;
 }
@@ -112,15 +115,27 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
 }
 
 void
-utmCallback(const message &msg, State &mu)
+utmCallback(const message &msg, State &mu, const Quaterniond &q)
 {
-    if (!seenutm || !seencov || !seenpva) {
+    Vector3d tip;
+    tip << 0.6*3.43, 0, -0.34;
+    tip = q._transformVector(tip);
+    if ((!seenutm || !seencov) && seenpva) {
         seenutm=true;
-        mu.enu(msg.utm);
-    } else { // Perform Z measurement
-        Matrix<double,1,1> z;
-        z << -msg.utm.up;
-        mu.update(z);
+        UTM utm_water;
+        utm_water.northing = msg.utm.northing;
+        utm_water.easting = msg.utm.easting;
+        utm_water.up = -tip[2];
+        utm_water.zone_i = msg.utm.zone_i;
+        utm_water.zone_c = msg.utm.zone_c;
+        mu.enu(utm_water);
+    } else if (seenutm) { // Perform Z measurement
+        std::vector<measurement_t> z;
+        measurement_t height;
+        height.z_type = HEIGHT;
+        height.height = tip[2];
+        z.push_back(height);
+        //mu.update(q,z);
     }
     return;
 }
@@ -268,7 +283,7 @@ while (scanf("%s", line)!=EOF) {
     
     switch ( msg.msg_type ) {
         case BESTUTM:	
-            utmCallback(msg, mu);
+            utmCallback(msg, mu,qbw);
             break;
 
         case IMG: