testcommit

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: equidistant
+  distortion_model: pre-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: equidistant
+  distortion_model: pre-equidistant
   intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
   resolution: [512, 512]
   rostopic: /cam1/image_raw

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: equidistant
+  distortion_model: pre-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: equidistant
+  distortion_model: pre-equidistant
   intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
   resolution: [1024, 1024]
   rostopic: /cam1/image_raw

include/msckf_vio/feature.hpp

@@ -6,7 +6,7 @@
  */
 
 #ifndef MSCKF_VIO_FEATURE_H
-#define MSCKF_VIO_FEATURE_H
+#define MSCKF_VIO_FEATURE_Hs
 
 #include <iostream>
 #include <map>
@@ -935,7 +935,7 @@ bool Feature::VisualizePatch(
   //cv::imwrite(loc.str(), cam0.featureVisu);
 
   cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(1);
+  cvWaitKey(0);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const

include/msckf_vio/msckf_vio.h

@@ -220,12 +220,14 @@ class MsckfVio {
         const Feature& feature,
         const StateIDType&  cam_state_id,
         Eigen::MatrixXd& H_xl,
-        Eigen::MatrixXd& H_yl);
+        Eigen::MatrixXd& H_yl,
+        int patchsize);
 
     bool PhotometricPatchPointResidual(
         const StateIDType& cam_state_id,
         const Feature& feature, 
-        Eigen::VectorXd& r);
+        Eigen::VectorXd& r,
+        int patchsize);
 
     bool PhotometricPatchPointJacobian(
         const CAMState& cam_state,
@@ -233,6 +235,7 @@ class MsckfVio {
         const Feature& feature,
         Eigen::Vector3d point,
         int count,
+        int patchsize,
         Eigen::Matrix<double, 2, 1>& H_rhoj,
         Eigen::Matrix<double, 2, 6>& H_plj,
         Eigen::Matrix<double, 2, 6>& H_pAj,
@@ -243,7 +246,8 @@ class MsckfVio {
         const FeatureIDType& feature_id,
         Eigen::MatrixXd& H_x,
         Eigen::MatrixXd& H_y,
-        Eigen::VectorXd& r);
+        Eigen::VectorXd& r,
+        int patchsize);
 
     void twodotFeatureJacobian(
         const FeatureIDType& feature_id,
@@ -253,7 +257,8 @@ class MsckfVio {
     bool PhotometricFeatureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
-        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r,
+        int patchsize);
 
     void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
     void measurementUpdate(const Eigen::MatrixXd& H,

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="5"/>
-      <param name="grid_max_feature_num" value="10"/>
+      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_max_feature_num" value="4"/>
       <param name="pyramid_levels" value="3"/>
       <param name="patch_size" value="15"/>
       <param name="fast_threshold" value="10"/>
@@ -24,9 +24,9 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu0"/>
-      <remap from="~cam0_image" to="/cam0/image_raw"/>
-      <remap from="~cam1_image" to="/cam1/image_raw"/>
+      <remap from="~imu" to="/imu02"/>
+      <remap from="~cam0_image" to="/cam0/image_raw2"/>
+      <remap from="~cam1_image" to="/cam1/image_raw2"/>
 
     </node>
   </group>

launch/msckf_vio_tinytum.launch

@@ -22,7 +22,7 @@
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="5"/>

launch/msckf_vio_tum.launch

@@ -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"/>

src/image_processor.cpp

@@ -221,9 +221,13 @@ void ImageProcessor::stereoCallback(
 
   ros::Time start_time = ros::Time::now();
 
-  image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  //ROS_INFO("Publishing: %f",
+  cv::Mat newImage;
+  image_handler::undistortImage(cam0_curr_img_ptr->image, newImage, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  newImage.copyTo(cam0_curr_img_ptr->image);
+  image_handler::undistortImage(cam1_curr_img_ptr->image, newImage, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  newImage.copyTo( cam1_curr_img_ptr->image);
+
+//ROS_INFO("Publishing: %f",
   //    (ros::Time::now()-start_time).toSec());
   // Build the image pyramids once since they're used at multiple places
   createImagePyramids();

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.05;
+double Feature::observation_noise = 0.2;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -541,14 +541,15 @@ void MsckfVio::manageMovingWindow(
       sensor_msgs::image_encodings::MONO8);
   cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
-
-  image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-
-
+  
+  cv::Mat newImage0;
+  cv::Mat newImage1;
+  image_handler::undistortImage(cam0_img_ptr->image, newImage0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_img_ptr->image, newImage1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  
   // save image information into moving window
-  cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
-  cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
+  cam0.moving_window[state_server.imu_state.id].image = newImage0.clone();
+  cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
 
   cv::Mat xder;
   cv::Mat yder;
@@ -1561,9 +1562,9 @@ void MsckfVio::twodotFeatureJacobian(
 bool MsckfVio::PhotometricPatchPointResidual(
   const StateIDType& cam_state_id,
   const Feature& feature, 
-  VectorXd& r)
+  VectorXd& r, int patchsize)
 {
-  VectorXd r_photo = VectorXd::Zero(2*N*N);
+  VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
   int count = 0;
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
 
@@ -1574,18 +1575,19 @@ bool MsckfVio::PhotometricPatchPointResidual(
   std::vector<double> photo_z_c0, photo_z_c1;
 
   // estimate irradiance based on anchor frame
-  
+ 
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
   for (auto& estimate_irradiance_j : estimate_irradiance)
             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;
   cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
@@ -1596,14 +1598,31 @@ bool MsckfVio::PhotometricPatchPointResidual(
 
   cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image;
   cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image;
-  for(int i = 0; i<N; i++) {
-    for(int j = 0; j<N ; j++) {
-      true_irradiance_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(N-1)/2), p_f_c0.y + (j-(N-1)/2)), current_image_c0));
-      true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(N-1)/2), p_f_c1.y + (j-(N-1)/2)), current_image_c1));
+  for(int i = 0; i<patchsize; i++) {
+    for(int j = 0; j<patchsize; j++) {
+      true_irradiance_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(patchsize-1)/2), p_f_c0.y + (j-(patchsize-1)/2)), current_image_c0));
+      true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(patchsize-1)/2), p_f_c1.y + (j-(patchsize-1)/2)), current_image_c1));
     }
   }
+ 
+  std::vector<Eigen::Vector3d> new_anchorPatch_3d;
+  if (patchsize == 3 and N == 5)
+  {
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
+  }
+  else
+  {
+    new_anchorPatch_3d = feature.anchorPatch_3d;
+  }
 
-    // get residual
   for(auto point : feature.anchorPatch_3d)
   {
 
@@ -1629,6 +1648,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
     count++;
   }
   r = r_photo;
+
   return true;
 }
 
@@ -1639,6 +1659,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
     const Feature& feature,
     Eigen::Vector3d point,
     int count,
+    int patchsize,
     Eigen::Matrix<double, 2, 1>& H_rhoj,
     Eigen::Matrix<double, 2, 6>& H_plj,
     Eigen::Matrix<double, 2, 6>& H_pAj,
@@ -1723,11 +1744,11 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   // d{}^Gp_P{ij} / \rho_i
   double rho = feature.anchor_rho;
   // Isometry T_anchor_w takes a vector in anchor frame to world frame
-  dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho));
+  dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho*rho), 1/(rho*rho));
 
   dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
-                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), 
-                                                 feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), 
+                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho), 
+                                                 feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho), 
                                                  1/(rho)));
   dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
@@ -1750,41 +1771,61 @@ bool MsckfVio::PhotometricPatchPointJacobian(
 bool MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r, int patchsize) 
 {
   // 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 r_photo = VectorXd::Zero(2*patchsize*patchsize);
 
-  // one line of the NxN Jacobians
+  // one line of the patchsizexpatchsize Jacobians
   Eigen::Matrix<double, 2, 1> H_rhoj;
   Eigen::Matrix<double, 2, 6> H_plj;
   Eigen::Matrix<double, 2, 6> H_pAj;
 
-  Eigen::MatrixXd dI_dh(2* N*N, 4);
+  Eigen::MatrixXd dI_dh(2* patchsize*patchsize, 4);
 
   // combined Jacobians
-  Eigen::MatrixXd H_rho(2 * N*N, 1);
-  Eigen::MatrixXd H_pl(2 * N*N, 6);
-  Eigen::MatrixXd H_pA(2 * N*N, 6);
+  Eigen::MatrixXd H_rho(2 * patchsize*patchsize, 1);
+  Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6);
+  Eigen::MatrixXd H_pA(2 * patchsize*patchsize, 6);
 
   // calcualte residual of patch
-  if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
+  if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo, patchsize))
+  {
     return false;
-
+  }
   //cout << "r\n" << r_photo  << endl;
   // calculate jacobian for patch
   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)
+
+  std::vector<Eigen::Vector3d> new_anchorPatch_3d;
+  if (patchsize == 3 and N == 5)
+  {
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
+      new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
+  }
+  else
+  {
+    new_anchorPatch_3d = feature.anchorPatch_3d;
+  }
+
+  for(auto point : new_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))
+    if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, patchsize, H_rhoj, H_plj, H_pAj, dI_dhj))
     {
         valid_count++;
         valid = true;
@@ -1796,7 +1837,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     H_pA.block<2, 6>(count*2, 0) = H_pAj;
 
     dI_dh.block<2, 4>(count*2, 0) = dI_dhj;
-
     count++;
   }
   // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
@@ -1807,7 +1847,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   MatrixXd H_xl;
   MatrixXd H_yl;
   
-  ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
+  ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl, patchsize);
 
   // set to return values
   H_x = H_xl;
@@ -1829,6 +1869,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     // visualizing functions
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
     feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
+    cout << "r\n" << r_photo << endl;
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
@@ -1845,16 +1886,17 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
                                   const Feature& feature,
                                   const StateIDType&  cam_state_id,
                                   Eigen::MatrixXd& H_xl,
-                                  Eigen::MatrixXd& H_yl)
+                                  Eigen::MatrixXd& H_yl,
+                                  int patchsize)
 {
-  H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
-  H_yl = MatrixXd::Zero(2*N*N, 1);
+  H_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7);
+  H_yl = MatrixXd::Zero(2*patchsize*patchsize, 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);
   // set anchor Jakobi
-  H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; 
+  H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*patchsize*patchsize, 6) = H_pA; 
 
   //get position of current frame in cam states
   auto cam_state_iter = state_server.cam_states.find(cam_state_id);
@@ -1862,7 +1904,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
   int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
   
   // set jakobi of state
-  H_xl.block(0, 21+cam_state_cntr*7, 2*N*N, 6) = H_pl;
+  H_xl.block(0, 21+cam_state_cntr*7, 2*patchsize*patchsize, 6) = H_pl;
 
   // set ones for irradiance bias
   // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
@@ -1882,7 +1924,8 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
 bool MsckfVio::PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
-    MatrixXd& H_x, VectorXd& r) 
+    MatrixXd& H_x, VectorXd& r,
+    int patchsize)
 {
 
   const auto& feature = map_server[feature_id];
@@ -1917,7 +1960,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
   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))
+    if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l, patchsize))
           continue;
 
     // set size of stacking jacobians, once the returned jacobians are known
@@ -1939,7 +1982,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
     r_i.segment(stack_cntr, r_l.size()) = r_l;
     stack_cntr += r_l.size();
   }
-
+ 
   // if not enough to propper nullspace (in paper implementation)
   if(stack_cntr < r_l.size())
      return false;
@@ -2590,6 +2633,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
+  /*
   if(gamma > 1000000)
   {
     cout << " logging " << endl;
@@ -2615,14 +2659,17 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
            << state_server.state_cov << endl;
     myfile.close();
   }
+  */
+
+  
 
   if (chi_squared_test_table[dof] == 0)
       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;
+    // cout << "passed" << endl;
     return true;
   } else {
     // cout << "failed" << endl;
@@ -2635,6 +2682,7 @@ void MsckfVio::removeLostFeatures() {
   // BTW, find the size the final Jacobian matrix and residual vector.
   int jacobian_row_size = 0;
   int pjacobian_row_size = 0;
+  int p2jacobian_row_size = 0;
   int twojacobian_row_size = 0;
 
   vector<FeatureIDType> invalid_feature_ids(0);
@@ -2676,6 +2724,7 @@ void MsckfVio::removeLostFeatures() {
     }
 
     pjacobian_row_size += 2*N*N*feature.observations.size();
+    p2jacobian_row_size += 2*3*3*feature.observations.size();
     twojacobian_row_size += 4*feature.observations.size();
     jacobian_row_size += 4*feature.observations.size() - 3;
 
@@ -2704,6 +2753,10 @@ void MsckfVio::removeLostFeatures() {
   VectorXd pr = VectorXd::Zero(pjacobian_row_size);
   int pstack_cntr = 0;
 
+  MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size,
+      21+7*state_server.cam_states.size());
+  VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
+  int p2stack_cntr = 0;
 
   MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size,
       21+7*state_server.cam_states.size());
@@ -2720,12 +2773,17 @@ void MsckfVio::removeLostFeatures() {
 
     MatrixXd H_xj;
     VectorXd r_j;
-    MatrixXd pH_xj;
-    VectorXd pr_j;
+    
     MatrixXd twoH_xj;
     VectorXd twor_j;
-    
-    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
+
+    MatrixXd pH_xj;
+    VectorXd pr_j;
+
+    MatrixXd p2H_xj;
+    VectorXd p2r_j;
+
+    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
@@ -2734,6 +2792,18 @@ 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;
+        p2stack_cntr += p2H_xj.rows();
+      }
+    }    
+    */
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
@@ -2831,6 +2901,7 @@ void MsckfVio::pruneLastCamStateBuffer()
   // Set the size of the Jacobian matrix.
   int jacobian_row_size = 0;
   int pjacobian_row_size = 0;
+  int p2jacobian_row_size = 0;
   int twojacobian_row_size = 0;
 
   
@@ -2869,6 +2940,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     }
 
     pjacobian_row_size += 2*N*N*feature.observations.size();
+    p2jacobian_row_size += 2*3*3*feature.observations.size();
     jacobian_row_size += 4*feature.observations.size() - 3;
     twojacobian_row_size += 4*feature.observations.size();
     
@@ -2884,11 +2956,16 @@ void MsckfVio::pruneLastCamStateBuffer()
   VectorXd pr = VectorXd::Zero(pjacobian_row_size);
   MatrixXd twoH_xj;
   VectorXd twor_j;
+  MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
+  VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
+  MatrixXd p2H_xj;
+  VectorXd p2r_j;
   MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
   VectorXd twor = VectorXd::Zero(twojacobian_row_size);
   int stack_cntr = 0;
   int pruned_cntr = 0;
   int pstack_cntr = 0;
+  int p2stack_cntr = 0;
   int twostack_cntr = 0;
 
   for (auto& item : map_server) {
@@ -2904,17 +2981,26 @@ void MsckfVio::pruneLastCamStateBuffer()
     for (const auto& cam_state : state_server.cam_states)
       involved_cam_state_ids.push_back(cam_state.first);
 
-
-    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
+    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
     {
-
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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 << "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)) {
+          p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
+          p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
+          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);
 
@@ -3003,6 +3089,7 @@ void MsckfVio::pruneCamStateBuffer() {
   // Find the size of the Jacobian matrix.
   int jacobian_row_size = 0;
   int pjacobian_row_size = 0;
+  int p2jacobian_row_size = 0;
   int twojacobian_row_size = 0;
 
   for (auto& item : map_server) {
@@ -3047,9 +3134,10 @@ void MsckfVio::pruneCamStateBuffer() {
         continue;
       }
     }
-    
+
     twojacobian_row_size += 4*involved_cam_state_ids.size();
     pjacobian_row_size += 2*N*N*involved_cam_state_ids.size();
+    p2jacobian_row_size += 2*3*3*involved_cam_state_ids.size();
     jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
   }
 
@@ -3061,11 +3149,19 @@ void MsckfVio::pruneCamStateBuffer() {
   MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size());
   VectorXd r = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
+  
   MatrixXd pH_xj;
   VectorXd pr_j;
   MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size());
   VectorXd pr = VectorXd::Zero(pjacobian_row_size);
   int pstack_cntr = 0;
+  
+  MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
+  VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
+  MatrixXd p2H_xj;
+  VectorXd p2r_j;
+  int p2stack_cntr = 0;
+
   MatrixXd twoH_xj;
   VectorXd twor_j;
   MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
@@ -3084,14 +3180,27 @@ 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(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
     { 
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// 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();
+          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 << "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)) {
+        p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
+        p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
+        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);