From 9c7f67d2fdcd202a540682ba9f0b29da9b5aa352 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Thu, 16 May 2019 13:56:37 +0200
Subject: [PATCH] minor print changes

---
 launch/msckf_vio_tum.launch |  2 +-
 src/msckf_vio.cpp           | 42 +++++++++++++++++++------------------
 2 files changed, 23 insertions(+), 21 deletions(-)

diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index c3a91d9..eca1ebc 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -21,7 +21,7 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="7"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index fb9bdde..31a891e 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -427,22 +427,19 @@ void MsckfVio::imageCallback(
   batchImuProcessing(feature_msg->header.stamp.toSec());
   
   // save true state in seperate state vector
-  
-  //if(ErrState)
-  //{
-    batchTruthProcessing(feature_msg->header.stamp.toSec());
-    
-    if(GROUNDTRUTH)
-    {
-      state_server.imu_state.position = true_state_server.imu_state.position;
-      state_server.imu_state.orientation = true_state_server.imu_state.orientation;
-      state_server.imu_state.position_null = true_state_server.imu_state.position_null;
-      state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
 
-      state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
-      state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
-    }
-  //}
+  batchTruthProcessing(feature_msg->header.stamp.toSec());
+  
+  if(GROUNDTRUTH)
+  {
+    state_server.imu_state.position = true_state_server.imu_state.position;
+    state_server.imu_state.orientation = true_state_server.imu_state.orientation;
+    state_server.imu_state.position_null = true_state_server.imu_state.position_null;
+    state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
+
+    state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
+    state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
+  }
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -1484,6 +1481,14 @@ void MsckfVio::PhotometricFeatureJacobian(
   H_x = A_null_space.transpose() * H_xi;
   r = A_null_space.transpose() * r_i;
 
+  
+  ofstream myfile;
+  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+  myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
+  myfile.close();
+  cout << "---------- LOGGED -------- " << endl; 
+  
+
   if(PRINTIMAGES)
   {
     std::cout << "resume playback" << std::endl;
@@ -1537,10 +1542,7 @@ void MsckfVio::measurementJacobian(
   Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
   
   // original jacobi
-  //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
-  // my version of calculation
   dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
-  //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
   dpc0_dxc.rightCols(3) = -R_w_c0;
 
   Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
@@ -1626,13 +1628,13 @@ void MsckfVio::featureJacobian(
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
-  /*
+  
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
   myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
   myfile.close();
   cout << "---------- LOGGED -------- " << endl; 
-  */
+  
   nh.setParam("/play_bag", false);
 
   return;