fixed minor error when not enough samples, edited feature ammount and patch size to make irradiance msckf more stable

config/camchain-imucam-tum-scaled.yaml

@@ -7,7 +7,7 @@ cam0:
   camera_model: pinhole
   distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
     0.00020293673591811182]
-  distortion_model: pre-equidistant
+  distortion_model: equidistant
   intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
   resolution: [512, 512]
   rostopic: /cam0/image_raw
@@ -25,7 +25,7 @@ cam1:
   camera_model: pinhole
   distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
     0.0003299517423931039]
-  distortion_model: pre-equidistant
+  distortion_model: equidistant
   intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
   resolution: [512, 512]
   rostopic: /cam1/image_raw

include/msckf_vio/feature.hpp

@@ -208,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
   bool estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimatedIllumination) const;
 
@@ -549,7 +549,7 @@ return delta;
 bool Feature::estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimated_illumination) const
 {
@@ -558,11 +558,11 @@ bool Feature::estimate_FrameIrradiance(
   // muliply by a and add b of this frame
 
   auto anchor = observations.begin();
-  if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
+  if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
     return false;
 
-  double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
+  double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms;
-  double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
+  double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms;
 
 
   double a_A = anchorExposureTime_ms;

launch/image_processor_tinytum.launch

@@ -14,8 +14,8 @@
       <rosparam command="load" file="$(arg calibration_file)"/>
       <param name="grid_row" value="4"/>
       <param name="grid_col" value="4"/>
-      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_min_feature_num" value="5"/>
-      <param name="grid_max_feature_num" value="4"/>
+      <param name="grid_max_feature_num" value="10"/>
       <param name="pyramid_levels" value="3"/>
       <param name="patch_size" value="15"/>
       <param name="fast_threshold" value="10"/>

launch/msckf_vio_euroc.launch

@@ -18,7 +18,7 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
+      <param name="FILTER" value="0"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>

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="3"/>
+      <param name="patch_size_n" value="5"/>
       <!-- 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="10"/>
+      <param name="max_cam_state_size" value="20"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>

src/msckf_vio.cpp

@@ -277,9 +277,9 @@ bool MsckfVio::createRosIO() {
   truth_sub = nh.subscribe("ground_truth", 100,
       &MsckfVio::truthCallback, this);
 
-  cam0_img_sub.subscribe(nh, "cam0_image", 10);
+  cam0_img_sub.subscribe(nh, "cam0_image", 100);
-  cam1_img_sub.subscribe(nh, "cam1_image", 10);
+  cam1_img_sub.subscribe(nh, "cam1_image", 100);
-  feature_sub.subscribe(nh, "features", 10);
+  feature_sub.subscribe(nh, "features", 100);
 
   image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub);
   image_sub.registerCallback(&MsckfVio::imageCallback, this);
@@ -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.4);
+      boost::math::quantile(chi_squared_dist, 0.05);
   }
 
   if (!createRosIO()) return false;
@@ -703,7 +703,7 @@ bool MsckfVio::resetCallback(
   is_first_img = true;
 
   // Restart the subscribers.
-  imu_sub = nh.subscribe("imu", 100,
+  imu_sub = nh.subscribe("imu", 1000,
       &MsckfVio::imuCallback, this);
 
   truth_sub = nh.subscribe("ground_truth", 100,
@@ -1563,25 +1563,28 @@ bool MsckfVio::PhotometricPatchPointResidual(
   const Feature& feature, 
   VectorXd& r)
 {
-
   VectorXd r_photo = VectorXd::Zero(2*N*N);
   int count = 0;
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
 
   //estimate photometric measurement
   std::vector<double> estimate_irradiance;
-  std::vector<double> estimate_photo_z;
+  IlluminationParameter estimated_illumination;
+  std::vector<double> estimate_photo_z_c0, estimate_photo_z_c1;
   std::vector<double> photo_z_c0, photo_z_c1;
 
   // estimate irradiance based on anchor frame
-  /*
+  
-  IlluminationParameter estimated_illumination;
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
   for (auto& estimate_irradiance_j : estimate_irradiance)
-            estimate_photo_z.push_back (estimate_irradiance_j);// * 
+            estimate_photo_z_c0.push_back (estimate_irradiance_j);// * 
+                    //estimated_illumination.frame_gain * estimated_illumination.feature_gain +
+                    //estimated_illumination.frame_bias + estimated_illumination.feature_bias);
+  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination);
+  for (auto& estimate_irradiance_j : estimate_irradiance)
+            estimate_photo_z_c1.push_back (estimate_irradiance_j);// * 
                     //estimated_illumination.frame_gain * estimated_illumination.feature_gain +
                     //estimated_illumination.frame_bias + estimated_illumination.feature_bias);
-  */
 
   // irradiance measurement around feature point in c0 and c1
   std::vector<double> true_irradiance_c0, true_irradiance_c1;
@@ -1615,8 +1618,10 @@ bool MsckfVio::PhotometricPatchPointResidual(
     // add observation
     photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0));
     photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1));
+    
     // calculate photom. residual acc. to paper
-    // r_photo(count) = photo_z[count] - estimate_photo_z[count];
+    //r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count];
+    //r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[count];
     
     // calculate photom. residual alternating between frames
     r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count];
@@ -1747,13 +1752,12 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     const FeatureIDType& feature_id,
     MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
 {
-
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
   //photometric observation
-  VectorXd r_photo = VectorXd::Zero(N*N);
+  VectorXd r_photo;
 
   // one line of the NxN Jacobians
   Eigen::Matrix<double, 2, 1> H_rhoj;
@@ -1771,7 +1775,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
     return false;
 
-
   //cout << "r\n" << r_photo  << endl;
   // calculate jacobian for patch
   int count = 0;
@@ -1796,13 +1799,13 @@ bool MsckfVio::PhotometricMeasurementJacobian(
 
     count++;
   }
-  cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
+  // 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;
+  // 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;
+  // cout << "h photo: \n" << h_photo << endl;
 
   // construct the jacobian structure needed for nullspacing
-  MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
+  MatrixXd H_xl;
-  MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1);
+  MatrixXd H_yl;
   
   ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
 
@@ -1844,6 +1847,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
                                   Eigen::MatrixXd& H_xl,
                                   Eigen::MatrixXd& H_yl)
 {
+  H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
+  H_yl = MatrixXd::Zero(2*N*N, 1);
+
   // get position of anchor in cam states
   auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
   int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
@@ -1869,7 +1875,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
     H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
   */  
 
-  H_yl = H_rho;
+  H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho;
 }
 
 
@@ -1894,23 +1900,35 @@ bool MsckfVio::PhotometricFeatureJacobian(
   }
 
   int jacobian_row_size = 0;
-  jacobian_row_size = 2 * N * N * valid_cam_state_ids.size();
  
-  MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
+  // stacked feature jacobians
-      21+state_server.cam_states.size()*7);
+  MatrixXd H_xi;
-  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1
+  MatrixXd H_yi;
-  VectorXd r_i = VectorXd::Zero(jacobian_row_size);
+  VectorXd r_i;
-  int stack_cntr = 0;
-
-  for (const auto& cam_id : valid_cam_state_ids) {
 
+  // temporary measurement jacobians
   MatrixXd H_xl;
   MatrixXd H_yl;
-    Eigen::VectorXd r_l = VectorXd::Zero(2*N*N);
+  Eigen::VectorXd r_l;
+  int stack_cntr = 0;
+  bool first = true;
 
+  // go through every valid state the feature was observed in
+  for (const auto& cam_id : valid_cam_state_ids) {
+
+    // skip observation if measurement is not valid
     if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
           continue;
 
+    // set size of stacking jacobians, once the returned jacobians are known
+    if(first)
+    {
+      first = not first;
+      jacobian_row_size = r_l.size() * valid_cam_state_ids.size();
+      H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols());
+      H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1
+      r_i = VectorXd::Zero(jacobian_row_size);
+    }
     auto cam_state_iter = state_server.cam_states.find(cam_id);
     int cam_state_cntr = std::distance(
         state_server.cam_states.begin(), cam_state_iter);
@@ -1918,12 +1936,12 @@ bool MsckfVio::PhotometricFeatureJacobian(
     // Stack the Jacobians.
     H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
     H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
-    r_i.segment(stack_cntr, 2*N*N) = r_l;
+    r_i.segment(stack_cntr, r_l.size()) = r_l;
-    stack_cntr += 2*N*N;
+    stack_cntr += r_l.size();
   }
 
   // if not enough to propper nullspace (in paper implementation)
-  if(stack_cntr < 2*N*N)
+  if(stack_cntr < r_l.size())
      return false;
 
   // Project the residual and Jacobians onto the nullspace
@@ -1936,6 +1954,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
     if(H_yi(i,0) != 0)
       valid = true;
 
+
   FullPivLU<MatrixXd> lu(H_yi.transpose());
   MatrixXd A_null_space = lu.kernel();
 
@@ -2004,7 +2023,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
     cout << "---------- LOGGED -------- " << endl; 
   }
 
+  if(valid)
     return true;
+  return false;
 }
 
 void MsckfVio::measurementJacobian(
@@ -2435,10 +2456,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
   if (H.rows() == 0 || r.rows() == 0)
-  {
-    cout << "zero" << endl;
       return;
-  }
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -2572,12 +2590,6 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-  // cout << "r" << r << endl;
-  // cout << "procov" << P1+P2 << endl;
-    if(filter == 1)
-    cout << "gate: " << dof << " " << gamma << " " <<
-      chi_squared_test_table[dof] << endl;
-
   if(gamma > 1000000)
   {
     cout << " logging " << endl;
@@ -2608,6 +2620,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
       return false;
   if (gamma < chi_squared_test_table[dof]) {
     // cout << "passed" << endl;
+    if(filter == 1)
+    cout << "gate: " << dof << " " << gamma << " " <<
+      chi_squared_test_table[dof] << endl;
     return true;
   } else {
     // cout << "failed" << endl;
@@ -2713,14 +2728,12 @@ void MsckfVio::removeLostFeatures() {
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
     {
         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, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
@@ -2735,7 +2748,6 @@ void MsckfVio::removeLostFeatures() {
       twostack_cntr += twoH_xj.rows();
     }
     
-
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
     //if (stack_cntr > 1500) break;
@@ -2745,11 +2757,8 @@ void MsckfVio::removeLostFeatures() {
   {
     pH_x.conservativeResize(pstack_cntr, pH_x.cols());
     pr.conservativeResize(pstack_cntr);
-
     photometricMeasurementUpdate(pH_x, pr);
   }
-
-  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2898,8 +2907,8 @@ 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(), 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();
@@ -2926,16 +2935,13 @@ void MsckfVio::pruneLastCamStateBuffer()
       feature.observations.erase(cam_id);
   }
 
-
   if(pstack_cntr)
   {
     pH_x.conservativeResize(pstack_cntr, pH_x.cols());
     pr.conservativeResize(pstack_cntr);
-
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -3081,14 +3087,11 @@ 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(), 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;
         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);
 
@@ -3104,12 +3107,12 @@ void MsckfVio::pruneCamStateBuffer() {
       twostack_cntr += twoH_xj.rows();
     }
     
+
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
+
   }
   
-
-  
   if(pstack_cntr)
   {
     pH_x.conservativeResize(pstack_cntr, pH_x.cols());
@@ -3117,7 +3120,6 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);