Nice quatviz-2 result

4 files changed, 11 insertions, 9 deletions

diff --git a/src/camera.h b/src/camera.h
index ba80ea2..7a1e7ee 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -8,7 +8,7 @@
 
 #include "types.h"
 #define BINNING 0.5 // set the binning factor
-#define YAWCORRECT 1.5
+#define YAWCORRECT 1.0
 #define DOYAWCORRECT
 using Eigen::Matrix;
 using Eigen::Vector4d;
diff --git a/src/main.cpp b/src/main.cpp
index bd31da9..57960dd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -443,7 +443,7 @@ main(int argc, char **argv)
 #else      /* -----  not ROS_PUBLISH  ----- */
     // Print unicsv header
 #if STATESIZE==13
-    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+    printf("GPS-stamp,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #else
     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
@@ -463,6 +463,10 @@ main(int argc, char **argv)
                 utmCallback(msg, mu,qbw);
 #endif
                 //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
+                if (seenutm && seenpva && seencov) {
+                    printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
+                    mu.unicsv();
+                }
                 break;
 
             case IMG:	
@@ -491,8 +495,6 @@ main(int argc, char **argv)
                         geometry_msgs::PoseStamped msg;
                         ros::spinOnce();
 #else
-                    if (seenutm) mu.unicsv();
-                        //mu.unicsv();
 #endif
                     }
                 }
diff --git a/src/main.h b/src/main.h
index 38ee7b7..5159eb7 100644
--- a/src/main.h
+++ b/src/main.h
@@ -37,8 +37,8 @@
 #define ACCBIASY 0.01465135
 #define ACCBIASZ -1*-0.00709229
 
-#define CANOECENTER 0.88            /* center of gravity of canoe  */
-#define CANOEHEIGHT -0.37
+#define CANOECENTER 0.92            /* center of gravity of canoe  */
+#define CANOEHEIGHT -0.41
 //#define CANOEHEIGHT -0.75
 #define DOWNSAMPLE 1            /*  */
 #define HEIGHT_FROM_ATTITUDE    /* use the attitude to measure the height */
diff --git a/src/vision.cpp b/src/vision.cpp
index 07f292f..4b5ce98 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -149,9 +149,6 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z, const vec
 
     cv::goodFeaturesToTrack(gray, corners, 20, 0.1, 50, mask);
     for (auto i=corners.begin(); i!=corners.end(); ++i) {
-        measurement_t m;
-        m.id = _id++;
-        m.z_type = MONO;
         Vector3d xi;
         xi << i->x, i->y, 1.;
         bool tooClose = false;
@@ -163,6 +160,9 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z, const vec
             }
         }
         if (tooClose) continue;
+        measurement_t m;
+        m.id = _id++;
+        m.z_type = MONO;
         m.source = cam.img2body(xi);
         m.reflection << 0,0,1;
         getTemplate(m.patch,xi);