From 7b7e9662174370b83dc7c327f7e4b6f44f23ce3f Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Fri, 28 Jun 2019 12:13:02 +0200
Subject: [PATCH] added ground truth visualization

---
 include/msckf_vio/msckf_vio.h |  1 +
 launch/msckf_vio_tum.launch   |  2 +-
 src/msckf_vio.cpp             | 42 ++++++++++++++++++++++++++---------
 3 files changed, 33 insertions(+), 12 deletions(-)

diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h
index 49af3cd..f8e7d62 100644
--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@@ -328,6 +328,7 @@ class MsckfVio {
     // Subscribers and publishers
     ros::Subscriber imu_sub;
     ros::Subscriber truth_sub;
+    ros::Publisher truth_odom_pub;
     ros::Publisher odom_pub;
     ros::Publisher marker_pub;
     ros::Publisher feature_pub;
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index e8294ef..b7e9365 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -22,7 +22,7 @@
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="5"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index ff4c24b..4dacfce 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -261,6 +261,7 @@ bool MsckfVio::createRosIO() {
   nh.setParam("/play_bag", true);
 
   odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
+  truth_odom_pub = nh.advertise<nav_msgs::Odometry>("true_odom", 10);
   feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
       "feature_point_cloud", 10);
   marker_pub = nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);
@@ -351,11 +352,6 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
 void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){
   static bool first_truth_odom_msg = true;
 
-  // errorstate
-  /*if(not ErrorState)
-    return;
-  */
-
   // If this is the first mocap odometry messsage, set
   // the initial frame.
   if (first_truth_odom_msg) {
@@ -1429,7 +1425,7 @@ void MsckfVio::twodotFeatureJacobian(
   {
 
   ofstream myfile;
-  myfile.open("/home/raphael/dev/octave/log2octave");
+  myfile.open("/home/raphael/dev/octave/two2octave");
   myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
          << "# name: Hx\n"
          << "# type: matrix\n"
@@ -1450,6 +1446,13 @@ void MsckfVio::twodotFeatureJacobian(
          << "# columns: " << 1 << "\n"
          << r_i << endl;
 
+
+  myfile << "# name: A\n"
+         << "# type: matrix\n"
+         << "# rows: " << A_null_space.rows() << "\n"  
+         << "# columns: " << 1 << "\n"
+         << A_null_space << endl;
+
   myfile.close();
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
@@ -1633,7 +1636,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   if(PRINTIMAGES)
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
+    //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
@@ -1745,8 +1748,8 @@ void MsckfVio::PhotometricFeatureJacobian(
            << "# columns: " << 1 << "\n"
            << r_i << endl;
 
-    myfile << "# name: r\n"
-           << "# type: A\n"
+    myfile << "# name: A\n"
+           << "# type: matrix\n"
            << "# rows: " << A_null_space.rows() << "\n"  
            << "# columns: " << A_null_space.cols() << "\n"
            << A_null_space << endl;
@@ -1920,8 +1923,8 @@ void MsckfVio::featureJacobian(
            << "# columns: " << 1 << "\n"
            << r_j << endl;
 
-    myfile << "# name: r\n"
-           << "# type: A\n"
+    myfile << "# name: A\n"
+           << "# type: matrix\n"
            << "# rows: " << A.rows() << "\n"  
            << "# columns: " << A.cols() << "\n"
            << A << endl;
@@ -2915,6 +2918,23 @@ void MsckfVio::publish(const ros::Time& time) {
           T_i_c_tf, time, child_frame_id, "camera"));
   }
 
+  // Publish true odometry
+  // Ground truth odometry.
+  nav_msgs::Odometry truth_odom_msg;
+  truth_odom_msg.header.stamp = time;
+  truth_odom_msg.header.frame_id = fixed_frame_id;
+  truth_odom_msg.child_frame_id = child_frame_id+"_true";
+
+  Eigen::Isometry3d true_T_i_w = Eigen::Isometry3d::Identity();
+  true_T_i_w.linear() = quaternionToRotation(
+    true_state_server.imu_state.orientation).transpose();
+  true_T_i_w.translation() = true_state_server.imu_state.position;
+  tf::poseEigenToMsg(true_T_i_w, truth_odom_msg.pose.pose);
+  Eigen::Isometry3d true_T_b_w = IMUState::T_imu_body * true_T_i_w *
+    IMUState::T_imu_body.inverse();
+
+  tf::poseEigenToMsg(true_T_b_w, truth_odom_msg.pose.pose);
+  truth_odom_pub.publish(truth_odom_msg);
   // Publish the odometry
   nav_msgs::Odometry odom_msg;
   odom_msg.header.stamp = time;