added larger sobel filter in calculation - converges sometimes for a few seconds

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="10"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>

launch/msckf_vio_tum.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="0"/>
+      <param name="FILTER" value="1"/>
 
       <!-- 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="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
 
 // Static member variables in Feature class.
 FeatureIDType Feature::next_id = 0;
-double Feature::observation_noise = 0.01;
+double Feature::observation_noise = 0.05;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -312,7 +312,7 @@ bool MsckfVio::initialize() {
   for (int i = 1; i < 1000; ++i) {
     boost::math::chi_squared chi_squared_dist(i);
     chi_squared_test_table[i] =
-      boost::math::quantile(chi_squared_dist, 0.05);
+      boost::math::quantile(chi_squared_dist, 0.1);
   }
 
   if (!createRosIO()) return false;
@@ -555,19 +555,21 @@ void MsckfVio::manageMovingWindow(
   cv::Mat deeper_frame;
 
   cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
-  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
+  cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
-  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
+  cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
-  xder/=8.;
+  xder/=72.;
-  yder/=8.;
+  yder/=72.;
 
   cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
   cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
 
   cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
-  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
+  cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
-  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
+  cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
-  xder/=8.;
+  xder/=72.;
-  yder/=8.;
+  yder/=72.;
+
+  cvWaitKey(0);
 
   cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
   cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@@ -1679,7 +1681,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
   dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
 
-  cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
+  //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
 
   // add jacobian
   
@@ -1718,6 +1720,10 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj              + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6
   H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj  + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6
 
+  // check if any direction not large enough for eval
+  if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01)
+    return false;
+  
   // check if point nullspaceable
   if (H_rhoj(0, 0) != 0)
     return true;
@@ -1751,9 +1757,11 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   Eigen::MatrixXd H_pA(2 * N*N, 6);
 
   // calcualte residual of patch
-  PhotometricPatchPointResidual(cam_state_id, feature, r_photo);
+  if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
+    return false;
 
-  cout << "r\n" << r_photo  << endl;
+
+  //cout << "r\n" << r_photo  << endl;
   // calculate jacobian for patch
   int count = 0;
   bool valid = false;
@@ -2446,7 +2454,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
     r_thin = r;
   }
 
-
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;
   MatrixXd S = H_thin*P*H_thin.transpose() +
@@ -2536,12 +2543,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
   MatrixXd state_cov_fixed = (state_server.state_cov +
       state_server.state_cov.transpose()) / 2.0;
   state_server.state_cov = state_cov_fixed;
+
   return;
 }
 
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-  return true;
+
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
   MatrixXd P2 = Feature::observation_noise *
@@ -2549,12 +2557,40 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-    //cout << "gate: " << dof << " " << gamma << " " <<
+  // cout << "r" << r << endl;
-    //chi_squared_test_table[dof] << endl;
+  // cout << "procov" << P1+P2 << endl;
+    cout << "gate: " << dof << " " << gamma << " " <<
+      chi_squared_test_table[dof] << endl;
+
+  if(gamma > 1000000)
+  {
+    cout << " logging " << endl;
+    ofstream myfile;
+    myfile.open("/home/raphael/dev/octave/log2octave");
+    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+           << "# name: H\n"
+           << "# type: matrix\n"
+           << "# rows: " << H.rows() << "\n"
+           << "# columns: " << H.cols() << "\n"
+           << H << endl;
+
+    myfile << "# name: r\n"
+           << "# type: matrix\n"
+           << "# rows: " << r.rows() << "\n"  
+           << "# columns: " << 1 << "\n"
+           << r << endl;
+
+    myfile << "# name: C\n"
+           << "# type: matrix\n"
+           << "# rows: " << state_server.state_cov.rows() << "\n"  
+           << "# columns: " << state_server.state_cov.cols() << "\n"
+           << state_server.state_cov << endl;
+    myfile.close();
+  }
 
     if (chi_squared_test_table[dof] == 0)
       return false;
-  if (gamma < chi_squared_test_table[dof]) {
+  if (gamma < chi_squared_test_table[dof]*10) {
     // cout << "passed" << endl;
     return true;
   } else {
@@ -2661,13 +2697,14 @@ void MsckfVio::removeLostFeatures() {
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
-        //cout << "passed" << endl;
+        cout << "passed" << endl;
         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();
       }
     }
 
+    /*
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
@@ -2681,6 +2718,7 @@ void MsckfVio::removeLostFeatures() {
       twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
       twostack_cntr += twoH_xj.rows();
     }
+    */
 
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
@@ -2695,7 +2733,7 @@ void MsckfVio::removeLostFeatures() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-
+  /*
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2705,7 +2743,7 @@ void MsckfVio::removeLostFeatures() {
   // Perform the measurement update step.
   measurementUpdate(H_x, r);
   twoMeasurementUpdate(twoH_x, twor);
-
+  */
   // Remove all processed features from the map.
   for (const auto& feature_id : processed_feature_ids)
     map_server.erase(feature_id);
@@ -2845,13 +2883,13 @@ void MsckfVio::pruneLastCamStateBuffer()
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
-        //cout << "passed" << endl;
+        cout << "passed" << endl;
         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();
       }
     }
-
+    /*
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
@@ -2867,7 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
       twostack_cntr += twoH_xj.rows();
     }
-
+  */
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -2881,7 +2919,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-
+  /*
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2891,7 +2929,7 @@ void MsckfVio::pruneLastCamStateBuffer()
   // Perform the measurement update step.
   measurementUpdate(H_x, r);
   twoMeasurementUpdate(twoH_x, twor);
-
+  */
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
       state_server.cam_states.find(rm_cam_state_id));
@@ -3024,17 +3062,17 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
 
-    
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     { 
         if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
-        //cout << "passed" << endl;
+        cout << "passed" << endl;
         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();
       }
     }
 
+    /*
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
   
@@ -3049,7 +3087,7 @@ void MsckfVio::pruneCamStateBuffer() {
       twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
       twostack_cntr += twoH_xj.rows();
     }
-  
+    */
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -3063,6 +3101,7 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
+  /*
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -3072,7 +3111,7 @@ void MsckfVio::pruneCamStateBuffer() {
   // Perform the measurement update step.
   measurementUpdate(H_x, r);
   twoMeasurementUpdate(twoH_x, twor);
-
+  */
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),