removed dx filter, corrected jacobi calculation with bigger sobel (and correct division), removed scale for mahalanobis

include/msckf_vio/feature.hpp

@@ -736,6 +736,7 @@ bool Feature::VisualizeKernel(
 
   cvWaitKey(0);
 } 
+
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -934,7 +935,7 @@ bool Feature::VisualizePatch(
   //cv::imwrite(loc.str(), cam0.featureVisu);
 
   cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(0);
+  cvWaitKey(1);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const

include/msckf_vio/msckf_vio.h

@@ -261,7 +261,7 @@ class MsckfVio {
     void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
 
     bool gatingTest(const Eigen::MatrixXd& H,
-        const Eigen::VectorXd&r, const int& dof);
+        const Eigen::VectorXd&r, const int& dof, int filter=0);
     void removeLostFeatures();
     void findRedundantCamStates(
         std::vector<StateIDType>& rm_cam_state_ids);

launch/msckf_vio_euroc.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="2"/>
+      <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

@@ -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.1);
+      boost::math::quantile(chi_squared_dist, 0.4);
   }
 
   if (!createRosIO()) return false;
@@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow(
   cv::Mat yder;
   cv::Mat deeper_frame;
 
+  // generate derivative matrix for cam0
   cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
   cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
   cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
-  xder/=72.;
+  xder/=96.;
-  yder/=72.;
+  yder/=96.;
 
+  // save into moving window
   cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
   cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
 
+  // generate derivative matrix for cam 1
   cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
   cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
   cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
-  xder/=72.;
+  xder/=96.;
-  yder/=72.;
+  yder/=96.;
 
+
+  /*
+  cv::Mat norm_abs_xderImage;
+  cv::convertScaleAbs(xder, norm_abs_xderImage);
+  cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+  cv::imshow("xder", norm_abs_xderImage);
   cvWaitKey(0);
-
+  */
+  // save into moving window
   cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
   cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
 
@@ -1721,9 +1732,9 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   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)
+  /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1))
     return false;
-  
+  */
   // check if point nullspaceable
   if (H_rhoj(0, 0) != 0)
     return true;
@@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   int count = 0;
   bool valid = false;
   Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
+  int valid_count = 0;
   for (auto point : feature.anchorPatch_3d)
   {
     // get jacobi of single point in patch
     if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
+    {
+        valid_count++;
         valid = true;
+    }
 
     // stack point into entire jacobi
     H_rho.block<2, 1>(count*2, 0) = H_rhoj;
@@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
 
     count++;
   }
-
+  cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
   //Eigen::Matrix<double, 2, 1> h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo;
   //cout << "h photo: \n" << h_photo << endl;
 
@@ -2548,7 +2563,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 }
 
 
-bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
+bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) {
 
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
@@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
 
   // cout << "r" << r << endl;
   // cout << "procov" << P1+P2 << endl;
+    if(filter == 1)
     cout << "gate: " << dof << " " << gamma << " " <<
       chi_squared_test_table[dof] << endl;
 
@@ -2588,9 +2604,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
     myfile.close();
   }
 
-    if (chi_squared_test_table[dof] == 0)
+  if (chi_squared_test_table[dof] == 0)
       return false;
-  if (gamma < chi_squared_test_table[dof]*10) {
+  if (gamma < chi_squared_test_table[dof]) {
     // cout << "passed" << endl;
     return true;
   } else {
@@ -2696,7 +2712,7 @@ 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)) {
+        if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
         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;
@@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() {
       }
     }
 
-    /*
+    
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
@@ -2718,7 +2734,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.
@@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  /*
+  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2743,7 +2759,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);
@@ -2882,14 +2898,14 @@ 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)) {
+        if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
         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);
 
@@ -2905,7 +2921,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);
   }
@@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  /*
+  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2929,7 +2945,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));
@@ -3064,7 +3080,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
     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())) {
+        if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
         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;
@@ -3072,7 +3088,7 @@ void MsckfVio::pruneCamStateBuffer() {
       }
     }
 
-    /*
+    
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
   
@@ -3087,7 +3103,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);
   }
@@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  /*
+  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -3111,7 +3127,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(),