testcommit

README.md

@@ -1,22 +1,12 @@
 # MSCKF\_VIO
 
 
-The `MSCKF_VIO` package is a stereo-photometric version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
+The `MSCKF_VIO` package is a stereo version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
 
-This approach is based on the paper written by Ke Sun et al.
+The software is tested on Ubuntu 16.04 with ROS Kinetic.
-[https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036) and their Stereo MSCKF implementation, which tightly fuse the matched feature information of a stereo image pair into a 6DOF Pose.
-The approach implemented in this repository follows the semi-dense msckf approach tightly fusing the photometric
-information around the matched featues into the covariance matrix, as described and derived in the master thesis [Pose Estimation using a Stereo-Photometric Multi-State Constraint Kalman Filter](http://raphael.maenle.net/resources/sp-msckf/maenle_master_thesis.pdf).
-
-It's positioning is comparable to the approach from Ke Sun et al. with the photometric approach, with a higher
-computational load, especially with larger image patches around the feature. A video explaining the approach can be
-found on [https://youtu.be/HrqQywAnenQ](https://youtu.be/HrqQywAnenQ): 
-<br/>
-[![Stereo-Photometric MSCKF](https://img.youtube.com/vi/HrqQywAnenQ/0.jpg)](https://www.youtube.com/watch?v=HrqQywAnenQ)
-
-<br/>
-This software should be deployed using ROS Kinetic on Ubuntu 16.04 or 18.04.
 
+Video: [https://www.youtube.com/watch?v=jxfJFgzmNSw&t](https://www.youtube.com/watch?v=jxfJFgzmNSw&t=3s)<br/>
+Paper Draft: [https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036)
 
 ## License
 
@@ -38,6 +28,16 @@ cd your_work_space
 catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
+## Calibration
+
+An accurate calibration is crucial for successfully running the software. To get the best performance of the software, the stereo cameras and IMU should be hardware synchronized. Note that for the stereo calibration, which includes the camera intrinsics, distortion, and extrinsics between the two cameras, you have to use a calibration software. **Manually setting these parameters will not be accurate enough.** [Kalibr](https://github.com/ethz-asl/kalibr) can be used for the stereo calibration and also to get the transformation between the stereo cameras and IMU. The yaml file generated by Kalibr can be directly used in this software. See calibration files in the `config` folder for details. The two calibration files in the `config` folder should work directly with the EuRoC and [fast flight](https://github.com/KumarRobotics/msckf_vio/wiki) datasets. The convention of the calibration file is as follows:
+
+`camx/T_cam_imu`: takes a vector from the IMU frame to the camx frame.
+`cam1/T_cn_cnm1`: takes a vector from the cam0 frame to the cam1 frame.
+
+The filter uses the first 200 IMU messages to initialize the gyro bias, acc bias, and initial orientation. Therefore, the robot is required to start from a stationary state in order to initialize the VIO successfully.
+
+
 ## EuRoC and UPenn Fast flight dataset example usage
 
 First obtain either the [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) or the [UPenn fast flight](https://github.com/KumarRobotics/msckf_vio/wiki/Dataset) dataset.
@@ -75,8 +75,6 @@ To visualize the pose and feature estimates you can use the provided rviz config
 
 ## ROS Nodes
 
-The general structure is similar to the structure of the MSCKF implementation this repository is derived from.
-
 ### `image_processor` node
 
 **Subscribed Topics**

config/camchain-imucam-euroc.yaml

@@ -9,7 +9,7 @@ cam0:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
-  distortion_model: pre-radtan
+  distortion_model: radtan
   intrinsics: [458.654, 457.296, 367.215, 248.375]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0
@@ -26,7 +26,7 @@ cam1:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
-  distortion_model: pre-radtan
+  distortion_model: radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0

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>
@@ -137,7 +137,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
   inline bool checkMotion(
       const CamStateServer& cam_states) const;
 
-
   /*
    * @brief InitializeAnchor generates the NxN patch around the
    *        feature in the Anchor image
@@ -172,6 +171,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                   Eigen::Vector3d& in_p) const;
 
 
+
 /*
 *  @brief project PositionToCamera Takes a 3d position in a world frame
 *         and projects it into the passed camera frame using pinhole projection
@@ -224,7 +224,7 @@ bool VisualizeKernel(
   bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                   const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const;
   /*
@@ -713,22 +713,26 @@ bool Feature::VisualizeKernel(
   //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
 
 
-  // cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+  cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
-  // cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+  cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+
+
+  cv::Mat norm_abs_xderImage;
+  cv::normalize(abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+  cv::imshow("xder", norm_abs_xderImage);
+  cv::imshow("yder", abs_yderImage);
 
-  /*
   for(int i = 1; i < anchorImage.rows-1; i++)
     for(int j = 1; j < anchorImage.cols-1; j++)
       xderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x"));
 
-
   for(int i = 1; i < anchorImage.rows-1; i++)
     for(int j = 1; j < anchorImage.cols-1; j++)
       yderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
-  */
+  cv::imshow("anchor", anchorImage);
-  //cv::imshow("anchor", anchorImage);
+  cv::imshow("xder2", xderImage2);
-  cv::imshow("xder2", xderImage);
+  cv::imshow("yder2", yderImage2);
-  cv::imshow("yder2", yderImage);
 
   cvWaitKey(0);
 } 
@@ -736,7 +740,7 @@ bool Feature::VisualizeKernel(
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam,
+                  CameraCalibration& cam0,
                   const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const
 {
@@ -745,7 +749,7 @@ bool Feature::VisualizePatch(
 
   //visu - anchor
   auto anchor = observations.begin();
-  cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
   cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
   cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
 
@@ -757,10 +761,10 @@ bool Feature::VisualizePatch(
     cv::Point ys(point.x, point.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
   }  
-  cam.featureVisu = dottedFrame.clone(); 
+  cam0.featureVisu = dottedFrame.clone(); 
 
   // visu - feature
-  cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
+  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
   cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
   
   // set position in frame 
@@ -768,7 +772,7 @@ bool Feature::VisualizePatch(
   std::vector<double> projectionPatch;
   for(auto point : anchorPatch_3d)
   {
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
     projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
     // visu - feature
     cv::Point xs(p_in_c0.x, p_in_c0.y);
@@ -776,7 +780,7 @@ bool Feature::VisualizePatch(
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
   }  
 
-  cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
+  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
 
 
   // patches visualization
@@ -823,14 +827,10 @@ bool Feature::VisualizePatch(
   cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
-  cv::Point2f p_f;
+  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
-  if(cam.id == 0)
-    p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
-  else if(cam.id == 1)
-    p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3));
   // move to real pixels
+  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
 
-  p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
   for(int i = 0; i<N; i++)
   {
     for(int j = 0; j<N ; j++)
@@ -853,17 +853,17 @@ bool Feature::VisualizePatch(
 
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N; j++)
-      if(photo_r(2*(i*N+j))>0)
+      if(photo_r(i*N+j)>0)
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255),  
+                      cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),  
                       CV_FILLED);
       else
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), 
+                      cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), 
                       CV_FILLED);
 
   // gradient arrow
@@ -884,7 +884,7 @@ bool Feature::VisualizePatch(
                   3);
        */           
 
-  cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
+  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
       
 /*
   // visualize position of used observations and resulting feature position
@@ -916,15 +916,15 @@ bool Feature::VisualizePatch(
 
   // draw, x y position and arrow with direction - write z next to it
 
-  cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
+  cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
 
 
-  cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu);
+  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
 */
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
-  cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+  cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
   // create line?
 
@@ -932,10 +932,10 @@ bool Feature::VisualizePatch(
 
   std::stringstream loc;
  // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-  //cv::imwrite(loc.str(), cam.featureVisu);
+  //cv::imwrite(loc.str(), cam0.featureVisu);
 
-  cv::imshow("patch", cam.featureVisu);
+  cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(1);
+  cvWaitKey(0);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
@@ -979,6 +979,8 @@ cv::Point2f Feature::pixelDistanceAt(
   return distance;
 }
 
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -995,6 +997,7 @@ cv::Point2f Feature::projectPositionToCamera(
   Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Eigen::Vector3d& t_c0_w = cam_state.position;
 
+
   // project point according to model
   if(cam.id == 0)
   {
@@ -1046,9 +1049,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 
   auto anchor = observations.begin();
   if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
-  {
     return false;
-  }
+
   cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
   cv::Mat anchorImage_deeper;
   anchorImage.convertTo(anchorImage_deeper,CV_16S);
@@ -1071,16 +1073,17 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   
 
   // check if image has been pre-undistorted
-  if(cam.distortion_model.substr(0,3) == "pre")
+  if(cam.distortion_model.substr(0,3) == "pre-")
   {
+    std::cout << "is a pre" << std::endl;
     //project onto pixel plane
     undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
+
     // create vector of patch in pixel plane
     for(double u_run = -n; u_run <= n; u_run++)
       for(double v_run = -n; v_run <= n; v_run++)
         anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
 
-
     //project back into u,v
     for(int i = 0; i < N*N; i++)
       anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));

include/msckf_vio/image_processor.h

@@ -320,8 +320,6 @@ private:
     return;
   }
 
-  bool STREAMPAUSE;
-
   // Indicate if this is the first image message.
   bool is_first_img;
 

include/msckf_vio/msckf_vio.h

@@ -202,7 +202,7 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    bool featureJacobian(
+    void featureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -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,10 +246,10 @@ class MsckfVio {
         const FeatureIDType& feature_id,
         Eigen::MatrixXd& H_x,
         Eigen::MatrixXd& H_y,
-        Eigen::VectorXd& r);
+        Eigen::VectorXd& r,
+        int patchsize);
 
-
+    void twodotFeatureJacobian(
-    bool twodotFeatureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -254,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,
@@ -283,12 +287,10 @@ class MsckfVio {
     bool nan_flag;
     bool play;
     double last_time_bound;
-    double time_offset;
 
     // Patch size for Photometry
     int N;
-    // Image rescale
+
-    int SCALE; 
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 

launch/image_processor_tinytum.launch

@@ -11,14 +11,11 @@
       output="screen"
        >
 
-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-
       <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_max_feature_num" value="5"/>
+      <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"/>
@@ -27,10 +24,9 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu0"/>
+      <remap from="~imu" to="/imu02"/>
-      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam0_image" to="/cam0/image_raw2"/>
-      <remap from="~cam1_image" to="/cam1/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw2"/>
-
 
     </node>
   </group>

launch/image_processor_tum.launch

@@ -11,9 +11,6 @@
       output="screen"
        >
 
-      <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
-
       <rosparam command="load" file="$(arg calibration_file)"/>
       <param name="grid_row" value="4"/>
       <param name="grid_col" value="4"/>

launch/msckf_vio_euroc.launch

@@ -17,17 +17,15 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       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"/>
-      <param name="PrintImages" 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"/>
-      <param name="image_scale" value ="1"/>
 
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>

launch/msckf_vio_tinytum.launch

@@ -17,16 +17,15 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-      <param name="FILTER" value="0"/>
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="1"/>
 
       <!-- 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="3"/>
+      <param name="patch_size_n" value="5"/>
-      <param name="image_scale" value ="1"/>
-
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -34,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="12"/>
+      <param name="max_cam_state_size" value="20"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>

launch/msckf_vio_tum.launch

@@ -17,7 +17,6 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
       <param name="FILTER" value="1"/>
 
@@ -34,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/bagcontrol.py

@@ -1,64 +0,0 @@
-#!/usr/bin/env python
-import rosbag
-import rospy
-from sensor_msgs.msg import Imu, Image
-from geometry_msgs.msg import TransformStamped
-import time
-import signal
-import sys
-
-
-def signal_handler(sig, frame):
-        print('gracefully exiting the program.')
-        bag.close()
-        sys.exit(0)
-
-def main():
-    global bag
-
-    cam0_topic = '/cam0/image_raw'
-    cam1_topic = '/cam1/image_raw'
-    imu0_topic = '/imu0'
-    grnd_topic = '/vrpn_client/raw_transform'
-
-    rospy.init_node('controlbag')
-    rospy.set_param('play_bag', False)
-
-    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
-    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
-    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
-    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
-
-    signal.signal(signal.SIGINT, signal_handler)
-
-    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
-    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
-        
-        # pause if parameter set to false
-        flag = False
-        while not rospy.get_param('/play_bag'):
-            time.sleep(0.01)
-            if not flag:
-                print("stopped playback")
-                flag = not flag
-
-        if flag:
-            print("resume playback")
-
-        if topic == cam0_topic:
-            cam0_pub.publish(msg)
-
-        elif topic == cam1_topic:
-            cam1_pub.publish(msg)
-
-        elif topic == imu0_topic:
-            imu0_pub.publish(msg)
-
-        elif topic ==grnd_topic:
-            grnd_pub.publish(msg)
-
-        #print msg
-    bag.close()
-
-if __name__== "__main__":
-    main()

src/image_handler.cpp

@@ -40,12 +40,10 @@ void undistortImage(
       cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
   else if (distortion_model == "equidistant")
       src.copyTo(dst);
-
   else if (distortion_model == "pre-radtan")
       cv::undistort(src, dst, K, distortion_coeffs, K);
   else if (distortion_model == "radtan")
       src.copyTo(dst);
-
 }
 
 void undistortPoint(
@@ -97,7 +95,6 @@ void undistortPoint(
       pts_out.push_back(newPoint);
     }
    }
-
   else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
   {
     std::vector<cv::Point2f> temp_pts_out;

src/image_processor.cpp

@@ -42,9 +42,6 @@ ImageProcessor::~ImageProcessor() {
 }
 
 bool ImageProcessor::loadParameters() {
-
-  // debug parameters
-  nh.param<bool>("StreamPause", STREAMPAUSE, false);
   // Camera calibration parameters
   nh.param<string>("cam0/distortion_model",
       cam0.distortion_model, string("radtan"));
@@ -214,9 +211,7 @@ void ImageProcessor::stereoCallback(
     const sensor_msgs::ImageConstPtr& cam0_img,
     const sensor_msgs::ImageConstPtr& cam1_img) {
 
-    // stop playing bagfile if printing images
+  //cout << "==================================" << endl;
-  //if(STREAMPAUSE)
-   // nh.setParam("/play_bag_image", false);
 
   // Get the current image.
   cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -226,19 +221,14 @@ void ImageProcessor::stereoCallback(
 
   ros::Time start_time = ros::Time::now();
 
+  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);
 
-  cv::Mat new_cam0;
+//ROS_INFO("Publishing: %f",
-  cv::Mat new_cam1;
-
-  image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  
-  new_cam0.copyTo(cam0_curr_img_ptr->image);
-  new_cam1.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();
 
@@ -265,7 +255,6 @@ void ImageProcessor::stereoCallback(
     // Add new features into the current image.
     start_time = ros::Time::now();
     addNewFeatures();
-    
     //ROS_INFO("Addition time: %f",
     //    (ros::Time::now()-start_time).toSec());
 
@@ -294,12 +283,10 @@ void ImageProcessor::stereoCallback(
   //    (ros::Time::now()-start_time).toSec());
 
   // Update the previous image and previous features.
-  
   cam0_prev_img_ptr = cam0_curr_img_ptr;
   prev_features_ptr = curr_features_ptr;
   std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
 
-
   // Initialize the current features to empty vectors.
   curr_features_ptr.reset(new GridFeatures());
   for (int code = 0; code <
@@ -307,10 +294,6 @@ void ImageProcessor::stereoCallback(
     (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
   }
 
-    // stop playing bagfile if printing images
-  //if(STREAMPAUSE)
-  //  nh.setParam("/play_bag_image", true);
-
   return;
 }
 
@@ -607,7 +590,6 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
-
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -647,14 +629,9 @@ void ImageProcessor::stereoMatch(
     image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
                     cam0.distortion_coeffs, cam0_points_undistorted,
                     R_cam0_cam1);
-<<<<<<< HEAD
-    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
-                                cam1_distortion_model, cam1_distortion_coeffs);
-=======
 
     cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics,
                                 cam1.distortion_model, cam1.distortion_coeffs);
->>>>>>> photometry-jakobi
   }
 
   // Track features using LK optical flow method.
@@ -692,8 +669,6 @@ void ImageProcessor::stereoMatch(
 
   // Further remove outliers based on the known
   // essential matrix.
-
-
   vector<cv::Point2f> cam0_points_undistorted(0);
   vector<cv::Point2f> cam1_points_undistorted(0);
   image_handler::undistortPoints(
@@ -703,7 +678,6 @@ void ImageProcessor::stereoMatch(
       cam1_points, cam1.intrinsics, cam1.distortion_model,
       cam1.distortion_coeffs, cam1_points_undistorted);
 
-
   double norm_pixel_unit = 4.0 / (
       cam0.intrinsics[0]+cam0.intrinsics[1]+
       cam1.intrinsics[0]+cam1.intrinsics[1]);

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.2;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -137,8 +137,6 @@ bool MsckfVio::loadParameters() {
   // Get the photometric patch size
   nh.param<int>("patch_size_n",
       N, 3);
-  // Get rescaling factor for image
-  nh.param<int>("image_scale", SCALE, 1);
 
   // get camera information (used for back projection)
   nh.param<string>("cam0/distortion_model",
@@ -409,7 +407,6 @@ void MsckfVio::imageCallback(
   // stop playing bagfile if printing images
   if(STREAMPAUSE)
     nh.setParam("/play_bag", false);
-
   // Return if the gravity vector has not been set.
   if (!is_gravity_set) 
   {
@@ -427,8 +424,6 @@ void MsckfVio::imageCallback(
   // the origin.
   if (is_first_img) {
     is_first_img = false;
-
-    time_offset = feature_msg->header.stamp.toSec();
     state_server.imu_state.time = feature_msg->header.stamp.toSec();
   }
   static double max_processing_time = 0.0;
@@ -472,7 +467,6 @@ void MsckfVio::imageCallback(
 
 
   // cout << "4" << endl;
-
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
@@ -481,7 +475,7 @@ void MsckfVio::imageCallback(
 
   // cout << "5" << endl;
   start_time = ros::Time::now();
-  pruneCamStateBuffer();
+  pruneLastCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -548,15 +542,14 @@ void MsckfVio::manageMovingWindow(
   cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
   
-  cv::Mat new_cam0;
+  cv::Mat newImage0;
-  cv::Mat new_cam1;
+  cv::Mat newImage1;
-
+  image_handler::undistortImage(cam0_img_ptr->image, newImage0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_img_ptr->image, newImage1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
   
   // save image information into moving window
-  cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
+  cam0.moving_window[state_server.imu_state.id].image = newImage0.clone();
-  cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
+  cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
 
   cv::Mat xder;
   cv::Mat yder;
@@ -581,19 +574,20 @@ void MsckfVio::manageMovingWindow(
   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();
 
+
+
   //TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
   while(cam0.moving_window.size() > 100)
   {
@@ -793,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
 void MsckfVio::calcErrorState()
 {
 
-  // imu "error"
+  // imu error
   err_state_server.imu_state.id = state_server.imu_state.id;
   err_state_server.imu_state.time = state_server.imu_state.time;
 
@@ -839,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
 
   // Counter how many IMU msgs in the buffer are used.
   int used_truth_msg_cntr = 0;
-  double truth_time;
+
   for (const auto& truth_msg : truth_msg_buffer) {
-    truth_time = truth_msg.header.stamp.toSec();
+    double truth_time = truth_msg.header.stamp.toSec();
     if (truth_time < true_state_server.imu_state.time) {
       ++used_truth_msg_cntr;
       continue;
@@ -861,7 +855,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
     // Execute process model.
     processTruthtoIMU(truth_time, m_rotation, m_translation);
     ++used_truth_msg_cntr;
-
   }
   last_time_bound = time_bound;
 
@@ -872,31 +865,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
       truth_msg_buffer.begin()+used_truth_msg_cntr);
 
-  std::ofstream logfile;
-
-  int FileHandler;
-  char FileBuffer[1024];
-  float load = 0;
-
-  FileHandler = open("/proc/loadavg", O_RDONLY);
-  if( FileHandler >= 0) {
-    auto file  = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1);
-    sscanf(FileBuffer, "%f", &load);
-    close(FileHandler);
-  }
-
-  auto gt = true_state_server.imu_state.position;
-  auto gr = true_state_server.imu_state.orientation;
-  logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app);
-  //ros time, cpu load , ground truth, estimation, ros time
-  logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; ";
-  logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";";
-
-  // estimation
-  auto et = state_server.imu_state.position;
-  auto er = state_server.imu_state.orientation;
-  logfile << et[0] << "; "  << et[1] << "; "  << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;;  
-  logfile.close();
   /*
   // calculate change
   delta_position = state_server.imu_state.position - old_imu_state.position;
@@ -1492,13 +1460,11 @@ void MsckfVio::twodotMeasurementJacobian(
   return;
 }
 
-bool MsckfVio::twodotFeatureJacobian(
+void MsckfVio::twodotFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
-  if(FILTER != 2)
-    return false;
 
   const auto& feature = map_server[feature_id];
   
@@ -1520,9 +1486,7 @@ bool MsckfVio::twodotFeatureJacobian(
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
-
   MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
-
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1530,7 +1494,6 @@ bool MsckfVio::twodotFeatureJacobian(
 
     MatrixXd H_xl;
     MatrixXd H_yl;
-
     Eigen::VectorXd r_l = VectorXd::Zero(4);
 
     twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
@@ -1541,14 +1504,10 @@ bool MsckfVio::twodotFeatureJacobian(
     // 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, 4) = r_l;
     stack_cntr += 4;
   }
 
-
-  H_xi.conservativeResize(stack_cntr, H_xi.cols());
-  H_yi.conservativeResize(stack_cntr, H_yi.cols());
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
@@ -1596,16 +1555,16 @@ bool MsckfVio::twodotFeatureJacobian(
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
-  return true;
+  return;
 }
 
 
 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];
 
@@ -1622,6 +1581,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
             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);// * 
@@ -1638,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 i = 0; i<patchsize; i++) {
-    for(int j = 0; j<N ; j++) {
+    for(int j = 0; j<patchsize; 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_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-(N-1)/2), p_f_c1.y + (j-(N-1)/2)), current_image_c1));
+      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));
     }
   }
  
-    // get residual
+  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 : feature.anchorPatch_3d)
   {
 
@@ -1671,6 +1648,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
     count++;
   }
   r = r_photo;
+
   return true;
 }
 
@@ -1681,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,
@@ -1739,6 +1718,8 @@ 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;
+
   // add jacobian
   
   //dh / d{}^Cp_{ij}
@@ -1763,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), 
+                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho), 
-                                                 feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), 
+                                                 feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho), 
                                                  1/(rho)));
   dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
@@ -1790,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_rho(2 * patchsize*patchsize, 1);
-  Eigen::MatrixXd H_pl(2 * N*N, 6);
+  Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6);
-  Eigen::MatrixXd H_pA(2 * N*N, 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;
@@ -1836,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;
@@ -1847,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;
@@ -1868,7 +1868,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
 
     // visualizing functions
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss);
+    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);
   }
 
@@ -1885,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_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7);
-  H_yl = MatrixXd::Zero(2*N*N, 1);
+  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);
@@ -1902,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);
@@ -1922,10 +1924,9 @@ 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)
 {
-  if(FILTER != 1)
-    return false;
 
   const auto& feature = map_server[feature_id];
 
@@ -1959,10 +1960,8 @@ 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;
-      continue;
-    }
 
     // set size of stacking jacobians, once the returned jacobians are known
     if(first)
@@ -2138,15 +2137,12 @@ void MsckfVio::measurementJacobian(
   return;
 }
 
-bool MsckfVio::featureJacobian(
+void MsckfVio::featureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
-  
+    // stop playing bagfile if printing images
-
-  if(FILTER != 0)
-    return false;
 
   const auto& feature = map_server[feature_id];
 
@@ -2205,42 +2201,16 @@ bool MsckfVio::featureJacobian(
   FullPivLU<MatrixXd> lu(H_fj.transpose());
   MatrixXd A = lu.kernel();
 
-  H_x = A.transpose() * H_xj;
+   H_x = A.transpose() * H_xj;
-  r = A.transpose() * r_j;
+   r = A.transpose() * r_j;
 
-  Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r);
       // stop playing bagfile if printing images
   if(PRINTIMAGES)
   {
 
     ofstream myfile;
-    myfile.open("/home/raphael/dev/octave/org2octave");
+    myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
-           << "# name: Hx\n"
-           << "# type: matrix\n"
-           << "# rows: " << H_xj.rows() << "\n"
-           << "# columns: " << H_xj.cols() << "\n"
-           << H_xj << endl;
-
-    myfile << "# name: Hy\n"
-           << "# type: matrix\n"
-           << "# rows: " << H_fj.rows() << "\n"  
-           << "# columns: " << H_fj.cols() << "\n"
-           << H_fj << endl;
-
-    myfile << "# name: r\n"
-           << "# type: matrix\n"
-           << "# rows: " << r_j.rows() << "\n"  
-           << "# columns: " << 1 << "\n"
-           << r_j << endl;
-
-
-    myfile << "# name: xres\n"
-           << "# type: matrix\n"
-           << "# rows: " << xres.rows() << "\n"  
-           << "# columns: " << 1 << "\n"
-           << xres << endl;
-
     myfile.close();
     
     myfile.open("/home/raphael/dev/octave/org2octave");
@@ -2279,7 +2249,7 @@ bool MsckfVio::featureJacobian(
     myfile.close();
   }
 
-  return true;
+  return;
 
 }
 
@@ -2416,10 +2386,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
 
-  if (H.rows() == 0 || r.rows() == 0)
+   if (H.rows() == 0 || r.rows() == 0)
-  {
     return;
-  }
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -2665,12 +2633,43 @@ 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;
+    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]) {
-    // 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;
@@ -2683,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);
@@ -2724,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;
 
@@ -2752,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());
@@ -2768,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;
@@ -2782,25 +2792,31 @@ void MsckfVio::removeLostFeatures() {
       }
     }
     
-    if(featureJacobian(feature.id, cam_state_ids, H_xj, r_j))
+    /*
+    cout << "3: " << endl;
+    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
     {
-      if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
+        if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
-        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+        cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl; 
-        r.segment(stack_cntr, r_j.rows()) = r_j;
+        p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
-        stack_cntr += H_xj.rows();
+        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);
 
-    if(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j))
+    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;
-      if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
+      r.segment(stack_cntr, r_j.rows()) = r_j;
-        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      stack_cntr += H_xj.rows();
-        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+    }  
-        twostack_cntr += twoH_xj.rows();
+    if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
-      }  
+      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      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.
@@ -2813,21 +2829,15 @@ void MsckfVio::removeLostFeatures() {
     pr.conservativeResize(pstack_cntr);
     photometricMeasurementUpdate(pH_x, pr);
   }
+  H_x.conservativeResize(stack_cntr, H_x.cols());
+  r.conservativeResize(stack_cntr);
  
-  if(stack_cntr)
+  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  {
+  twor.conservativeResize(twostack_cntr);
-    H_x.conservativeResize(stack_cntr, H_x.cols());
-    r.conservativeResize(stack_cntr);
-    measurementUpdate(H_x, r);
- 
-  }
-  if(twostack_cntr)
-  {  
-    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-    twor.conservativeResize(twostack_cntr);
-    twoMeasurementUpdate(twoH_x, twor);
-  }
   
+  // 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)
@@ -2891,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;
 
   
@@ -2929,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();
     
@@ -2944,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) {
@@ -2964,10 +2981,8 @@ 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, N))
-    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)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
@@ -2975,23 +2990,31 @@ void MsckfVio::pruneLastCamStateBuffer()
       }
     }
 
-    if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
+    /*
+    cout << "3: " << endl;
+    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
-      if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
+        if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
-        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+          p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
-        r.segment(stack_cntr, r_j.rows()) = r_j;
+          p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
-        stack_cntr += H_xj.rows();
+          p2stack_cntr += p2H_xj.rows();
-        pruned_cntr++;
+        }
-      }
     }
+    */
+    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
+    twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
-    if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
+      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;
-      if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+      r.segment(stack_cntr, r_j.rows()) = r_j;
-        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      stack_cntr += H_xj.rows();
-        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+      pruned_cntr++;
-        twostack_cntr += twoH_xj.rows();
       }
+    
+    if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+      twostack_cntr += twoH_xj.rows();
     }
 
     for (const auto& cam_id : involved_cam_state_ids)
@@ -3005,19 +3028,15 @@ void MsckfVio::pruneLastCamStateBuffer()
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  if(stack_cntr)
+  H_x.conservativeResize(stack_cntr, H_x.cols());
-  {  
+  r.conservativeResize(stack_cntr);
-    H_x.conservativeResize(stack_cntr, H_x.cols());
-    r.conservativeResize(stack_cntr);
-    measurementUpdate(H_x, r);
-  }
  
-  if(twostack_cntr)
+  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  {  
+  twor.conservativeResize(twostack_cntr);
-    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+ 
-    twor.conservativeResize(twostack_cntr);
+  // Perform the measurement update step.
-    twoMeasurementUpdate(twoH_x, twor);
+  measurementUpdate(H_x, r);
-  }
+  twoMeasurementUpdate(twoH_x, twor);
   
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
@@ -3070,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) {
@@ -3117,6 +3137,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
     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;
   }
 
@@ -3128,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());
@@ -3151,31 +3180,43 @@ 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;
+          pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
-        pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
+          pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
-        pstack_cntr += pH_xj.rows();
+          pstack_cntr += pH_xj.rows();
       }
     }
-    if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
+
+    /*
+    cout << "3: " << endl;
+    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
-      if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
+      if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
-        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+        p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
-        r.segment(stack_cntr, r_j.rows()) = r_j;
+        p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
-        stack_cntr += H_xj.rows();
+        p2stack_cntr += p2H_xj.rows();
       }    
     }
-    if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
+    */
-    {
+
-      if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+    twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
-        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+
-        twostack_cntr += twoH_xj.rows();
+    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;
+      r.segment(stack_cntr, r_j.rows()) = r_j;
+      stack_cntr += H_xj.rows();
     }
 
+    if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      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);
 
@@ -3188,19 +3229,15 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  if(stack_cntr)
+  H_x.conservativeResize(stack_cntr, H_x.cols());
-  {  
+  r.conservativeResize(stack_cntr);
-    H_x.conservativeResize(stack_cntr, H_x.cols());
-    r.conservativeResize(stack_cntr);
-    measurementUpdate(H_x, r);
-  }
  
-  if(stack_cntr)
+  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  {  
+  twor.conservativeResize(twostack_cntr);
-    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+  
-    twor.conservativeResize(twostack_cntr);
+  // Perform the measurement update step.
-    twoMeasurementUpdate(twoH_x, twor);
+  measurementUpdate(H_x, r);
-  }
+  twoMeasurementUpdate(twoH_x, twor);
   
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {