From 55669ea2c96593045cd3867382e649aa6a511af7 Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Thu, 11 Jul 2019 16:16:25 +0200
Subject: [PATCH] minor edits

---
 launch/msckf_vio_tinytum.launch |  4 ++--
 src/msckf_vio.cpp               | 26 +++++++++-----------------
 2 files changed, 11 insertions(+), 19 deletions(-)

diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch
index 28ae944..f89ea7a 100644
--- a/launch/msckf_vio_tinytum.launch
+++ b/launch/msckf_vio_tinytum.launch
@@ -25,7 +25,7 @@
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -33,7 +33,7 @@
       <param name="frame_rate" value="20"/>
       <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
       <param name="child_frame_id" value="odom"/>
-      <param name="max_cam_state_size" value="20"/>
+      <param name="max_cam_state_size" value="12"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index fba0c54..590398e 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -449,7 +449,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "2" << endl;
+  // cout << "2" << endl;
   // Add new observations for existing features or new
   // features in the map server.
   start_time = ros::Time::now();
@@ -458,7 +458,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "3" << endl;
+  // cout << "3" << endl;
   // Add new images to moving window
   start_time = ros::Time::now();
   manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -466,14 +466,14 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "4" << endl;
+  // cout << "4" << endl;
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
   double remove_lost_features_time = (
       ros::Time::now()-start_time).toSec();
 
-  cout << "5" << endl;
+  // cout << "5" << endl;
   start_time = ros::Time::now();
   pruneLastCamStateBuffer();
   double prune_cam_states_time = (
@@ -483,7 +483,7 @@ void MsckfVio::imageCallback(
   batchTruthProcessing(feature_msg->header.stamp.toSec());
 
 
-  cout << "6" << endl;
+  // cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
   publish(feature_msg->header.stamp);
@@ -2064,9 +2064,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
     cout << "---------- LOGGED -------- " << endl; 
   }
 
-  cout << H_x.rows() << ":" << H_x.cols() << endl;
-  cout << r.rows() << ":" << r.cols() << endl;
-
   if(valid)
     return true;
   return false;
@@ -2662,13 +2659,14 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
   }
   */
 
-  if(filter == 1)
-    cout << "gate: " << dof << " " << gamma << " " <<
-      chi_squared_test_table[dof] << endl;
+  
 
   if (chi_squared_test_table[dof] == 0)
       return false;
   if (gamma < chi_squared_test_table[dof]) {
+    if(filter == 1)
+    cout << "gate: " << dof << " " << gamma << " " <<
+      chi_squared_test_table[dof] << endl;
     // cout << "passed" << endl;
     return true;
   } else {
@@ -2795,11 +2793,9 @@ void MsckfVio::removeLostFeatures() {
     
     
     cout << "3: " << endl;
-    
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
     {
         if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
-        
         cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl; 
         p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
         p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
@@ -2994,9 +2990,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       }
     }
 
-    /*
     cout << "3: " << endl;
-    
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
         if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@@ -3005,7 +2999,6 @@ void MsckfVio::pruneLastCamStateBuffer()
           p2stack_cntr += p2H_xj.rows();
         }
     }
-    */
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
@@ -3196,7 +3189,6 @@ void MsckfVio::pruneCamStateBuffer() {
     }
 
     cout << "3: " << endl;
-    
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
       if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {