Fixed initialization of quaternion covariance.

Previously, we were using deg^2 where it should have been rad^2.

1 files changed, 21 insertions, 2 deletions

diff --git a/src/main.cpp b/src/main.cpp
index e37c2a9..e7e08a5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -120,6 +120,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 #if STATESIZE==13
     Matrix3d Patt;
     Patt = msg.covariance.attitude;
+    // Convert deg^2 to rad^2
+    Patt *= M_PI*M_PI;
+    Patt /= (180.*180.);
     // Swap values for roll, pitch, yaw ordering
     Patt(0,0) = msg.covariance.attitude(1,1);
     Patt(1,1) = msg.covariance.attitude(0,0);
@@ -164,6 +167,8 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
         height.height = aboveWater(q);
         z.push_back(height);
 #endif
+        Vision viz;
+        viz.open(msg.image_names[0],cam);
 
         strcat(msg.image_names[0],".txt");
         FILE *fin;
@@ -192,8 +197,22 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
         if (fclose(fin)==EOF) {
             err_sys("fclose");
         }
+        // The measured values
+        //viz.drawMeasurements(z,cam, cv::Scalar(0,255,0), cv::Scalar(0,180,0),false);
+        // The predicted values
+        std::vector<measurement_t> ykk1;
+        std::vector<measurement_t> newFeatures;
+        viz.acquireFeatures(cam, newFeatures);
+        viz.drawMeasurements(newFeatures, cam, cv::Scalar(0,0,255), cv::Scalar(0,0,180),false);
 #if STATESIZE==13
-        mu.handle_measurements(z);
+        //mu.featuresAsMeasurements(ykk1);
+#else
+        //mu.featuresAsMeasurements(ykk1,q);
+#endif
+        //viz.drawMeasurements(ykk1, cam, cv::Scalar(255,0,0), cv::Scalar(180,0,0),true);
+        viz.show();
+#if STATESIZE==13
+        //mu.handle_measurements(z);
 #else
         mu.handle_measurements(z,q);
 #endif
@@ -484,7 +503,6 @@ main(int argc, char **argv)
 #else
                 utmCallback(msg, mu,qbw);
 #endif
-                if (seenutm) mu.unicsv();
                 //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                 break;
 
@@ -514,6 +532,7 @@ main(int argc, char **argv)
                         geometry_msgs::PoseStamped msg;
                         ros::spinOnce();
 #else
+                    if (seenutm) mu.unicsv();
                         //mu.unicsv();
 #endif
                     }