removed comments, reg. msckf not working currently

config/camchain-imucam-tum.yaml

@@ -7,7 +7,7 @@ cam0:
   camera_model: pinhole
   distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
     -0.001662284667857643]
-  distortion_model: pre-equidistant
+  distortion_model: equidistant
   intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
   resolution: [1024, 1024]
   rostopic: /cam0/image_raw
@@ -25,7 +25,7 @@ cam1:
   camera_model: pinhole
   distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
     -0.002347858896562788]
-  distortion_model: pre-equidistant
+  distortion_model: equidistant
   intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
   resolution: [1024, 1024]
   rostopic: /cam1/image_raw

launch/msckf_vio_tum.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="2"/>
+      <param name="FILTER" value="0"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="1"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -1547,7 +1547,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   int count = 0;
   double dx, dy;
 
-  cout << "patching" << endl;
   for (auto point : feature.anchorPatch_3d)
   {
     //cout << "____feature-measurement_____\n" << endl;
@@ -1666,7 +1665,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
-  cout << "returning" << endl;
   return true;
 }
 
@@ -1797,8 +1795,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
     cout << "---------- LOGGED -------- " << endl; 
   }
 
-  cout << "donefeature" << endl;
-
   return true;
 }
 
@@ -2166,8 +2162,6 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
   // Compute the error of the state.
 
   VectorXd delta_x = K * r;
-  cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
-  cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
   if (FILTER != 2) return;
 
@@ -2272,13 +2266,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
   // Compute the error of the state.
 
   VectorXd delta_x = K * r;
-  cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
-  cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
   if (FILTER != 1) return;
 
-  cout << "here" << endl;
-
   if(PRINTIMAGES)
   {
   //octave
@@ -2438,9 +2428,6 @@ void MsckfVio::removeLostFeatures() {
   //  processed_feature_ids.size() << endl;
   //cout << "jacobian row #: " << jacobian_row_size << endl;
 
-
-  cout << "sizing" << endl;
-
   // Remove the features that do not have enough measurements.
   for (const auto& feature_id : invalid_feature_ids)
     map_server.erase(feature_id);
@@ -2479,27 +2466,19 @@ void MsckfVio::removeLostFeatures() {
     MatrixXd twoH_xj;
     VectorXd twor_j;
 
-
+    /*
-    cout << "measuring" << endl;
-
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true);
     {
-            cout << "p gating" << endl;
-
       if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();
       }
-    }
+    }*/
 
-    cout << "donephoto" << endl;
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
-    cout << "two" << endl;
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
-    cout << "gating" << endl;
-
     if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -2524,7 +2503,6 @@ void MsckfVio::removeLostFeatures() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  cout << "resizing" << endl;
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2670,26 +2648,21 @@ void MsckfVio::pruneLastCamStateBuffer()
     for (const auto& cam_state : state_server.cam_states)
       involved_cam_state_ids.push_back(cam_state.first);
 
-    cout << "measuring" << endl;
+    /*
-
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true);
     {
-            cout << "p gating" << endl;
 
       if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();
       }
-    }
+    }*/
 
 
-    cout << "norm" << endl;
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-    cout << "two" << endl;
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
-    cout << "gating" << endl;
       if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -2857,23 +2830,18 @@ void MsckfVio::pruneCamStateBuffer() {
     }
 
     if (involved_cam_state_ids.size() == 0) continue;
-        cout << "measuring" << endl;
 
+    /*
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     { 
-      cout << "p gating" << endl;
       if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();
       }
-    }
+    }*/
-    cout << "norm" << endl;
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-    cout << "two" << endl;
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
-
-    cout << "gating" << endl;
     
     if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;