removed scaling

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: radtan
+  distortion_model: pre-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: radtan
+  distortion_model: pre-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: pre-equidistant
+  distortion_model: 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: pre-equidistant
+  distortion_model: 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_Hs
+#define MSCKF_VIO_FEATURE_H
 
 #include <iostream>
 #include <map>
@@ -137,6 +137,7 @@ 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
@@ -171,7 +172,6 @@ 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& cam0,
+                  CameraCalibration& cam,
                   const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const;
   /*
@@ -713,26 +713,22 @@ bool Feature::VisualizeKernel(
   //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
 
 
-  cv::Mat xderImage2(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);
+  // cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+  // cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
 
+  /*
   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("xder2", xderImage2);
-  cv::imshow("yder2", yderImage2);
+  */
+  //cv::imshow("anchor", anchorImage);
+  cv::imshow("xder2", xderImage);
+  cv::imshow("yder2", yderImage);
 
   cvWaitKey(0);
 } 
@@ -740,7 +736,7 @@ bool Feature::VisualizeKernel(
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
                   const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const
 {
@@ -749,7 +745,7 @@ bool Feature::VisualizePatch(
 
   //visu - anchor
   auto anchor = observations.begin();
-  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
   cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
   cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
 
@@ -761,10 +757,10 @@ bool Feature::VisualizePatch(
     cv::Point ys(point.x, point.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
   }  
-  cam0.featureVisu = dottedFrame.clone(); 
+  cam.featureVisu = dottedFrame.clone(); 
 
   // visu - feature
-  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+  cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
   cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
   
   // set position in frame 
@@ -772,7 +768,7 @@ bool Feature::VisualizePatch(
   std::vector<double> projectionPatch;
   for(auto point : anchorPatch_3d)
   {
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
     projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
     // visu - feature
     cv::Point xs(p_in_c0.x, p_in_c0.y);
@@ -780,7 +776,7 @@ bool Feature::VisualizePatch(
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
   }  
 
-  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+  cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
 
 
   // patches visualization
@@ -827,10 +823,14 @@ 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(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  cv::Point2f p_f;
+  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(i*N+j)>0)
+      if(photo_r(2*(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(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),  
+                      cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*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(i*N+j)*255, 255 + photo_r(i*N+j)*255), 
+                      cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), 
                       CV_FILLED);
 
   // gradient arrow
@@ -884,7 +884,7 @@ bool Feature::VisualizePatch(
                   3);
        */           
 
-  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
+  cv::hconcat(cam.featureVisu, irradianceFrame, cam.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(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
+  cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
 
 
-  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
+  cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu);
 */
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
-  cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+  cv::putText(cam.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(), cam0.featureVisu);
+  //cv::imwrite(loc.str(), cam.featureVisu);
 
-  cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(0);
+  cv::imshow("patch", cam.featureVisu);
+  cvWaitKey(1);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
@@ -979,8 +979,6 @@ cv::Point2f Feature::pixelDistanceAt(
   return distance;
 }
 
-
-
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -997,7 +995,6 @@ 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)
   {
@@ -1049,8 +1046,9 @@ 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);
@@ -1073,17 +1071,16 @@ 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]));
@@ -1119,7 +1116,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   for(auto point : anchorPatch_real)
     if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
       return false;
-    
+  
   for(auto point : anchorPatch_real)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
     

include/msckf_vio/image_processor.h

@@ -320,6 +320,8 @@ 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.
-    void featureJacobian(
+    bool featureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -220,14 +220,12 @@ class MsckfVio {
         const Feature& feature,
         const StateIDType&  cam_state_id,
         Eigen::MatrixXd& H_xl,
-        Eigen::MatrixXd& H_yl,
-        int patchsize);
+        Eigen::MatrixXd& H_yl);
 
     bool PhotometricPatchPointResidual(
         const StateIDType& cam_state_id,
         const Feature& feature, 
-        Eigen::VectorXd& r,
-        int patchsize);
+        Eigen::VectorXd& r);
 
     bool PhotometricPatchPointJacobian(
         const CAMState& cam_state,
@@ -235,7 +233,6 @@ 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,
@@ -246,10 +243,10 @@ class MsckfVio {
         const FeatureIDType& feature_id,
         Eigen::MatrixXd& H_x,
         Eigen::MatrixXd& H_y,
-        Eigen::VectorXd& r,
-        int patchsize);
+        Eigen::VectorXd& r);
 
-    void twodotFeatureJacobian(
+
+    bool twodotFeatureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -257,8 +254,7 @@ class MsckfVio {
     bool PhotometricFeatureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
-        Eigen::MatrixXd& H_x, Eigen::VectorXd& r,
-        int patchsize);
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
     void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
     void measurementUpdate(const Eigen::MatrixXd& H,
@@ -287,10 +283,12 @@ 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,11 +11,14 @@
       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="4"/>
+      <param name="grid_max_feature_num" value="5"/>
       <param name="pyramid_levels" value="3"/>
       <param name="patch_size" value="15"/>
       <param name="fast_threshold" value="10"/>
@@ -24,9 +27,10 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu02"/>
-      <remap from="~cam0_image" to="/cam0/image_raw2"/>
-      <remap from="~cam1_image" to="/cam1/image_raw2"/>
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
 
     </node>
   </group>

launch/image_processor_tum.launch

@@ -11,6 +11,9 @@
       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,15 +17,17 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="0"/>
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="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)"/>

launch/msckf_vio_tinytum.launch

@@ -17,15 +17,16 @@
       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)"/>
 
@@ -33,7 +34,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"/>

launch/msckf_vio_tum.launch

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

src/bagcontrol.py

@@ -0,0 +1,64 @@
+#!/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,10 +40,12 @@ 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(
@@ -95,6 +97,7 @@ 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,6 +42,9 @@ 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"));
@@ -211,7 +214,9 @@ void ImageProcessor::stereoCallback(
     const sensor_msgs::ImageConstPtr& cam0_img,
     const sensor_msgs::ImageConstPtr& cam1_img) {
 
-  //cout << "==================================" << endl;
+    // stop playing bagfile if printing images
+  //if(STREAMPAUSE)
+   // nh.setParam("/play_bag_image", false);
 
   // Get the current image.
   cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -221,14 +226,19 @@ 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);
 
-//ROS_INFO("Publishing: %f",
+  cv::Mat new_cam0;
+  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();
 
@@ -255,6 +265,7 @@ 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());
 
@@ -283,10 +294,12 @@ 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 <
@@ -294,6 +307,10 @@ 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;
 }
 
@@ -590,6 +607,7 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
+
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -669,6 +687,8 @@ 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(
@@ -678,6 +698,7 @@ 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.2;
+double Feature::observation_noise = 0.01;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -137,6 +137,8 @@ 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",
@@ -407,6 +409,7 @@ 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) 
   {
@@ -424,6 +427,8 @@ 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;
@@ -467,6 +472,7 @@ void MsckfVio::imageCallback(
 
 
   // cout << "4" << endl;
+
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
@@ -475,7 +481,7 @@ void MsckfVio::imageCallback(
 
   // cout << "5" << endl;
   start_time = ros::Time::now();
-  pruneLastCamStateBuffer();
+  pruneCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -541,15 +547,16 @@ void MsckfVio::manageMovingWindow(
       sensor_msgs::image_encodings::MONO8);
   cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
-  
-  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);
-  
+
+  cv::Mat new_cam0;
+  cv::Mat new_cam1;
+
+  image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.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 = newImage0.clone();
-  cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
+  cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
+  cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
 
   cv::Mat xder;
   cv::Mat yder;
@@ -574,20 +581,19 @@ 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)
   {
@@ -787,7 +793,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;
 
@@ -833,9 +839,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) {
-    double truth_time = truth_msg.header.stamp.toSec();
+    truth_time = truth_msg.header.stamp.toSec();
     if (truth_time < true_state_server.imu_state.time) {
       ++used_truth_msg_cntr;
       continue;
@@ -855,6 +861,7 @@ 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;
 
@@ -865,6 +872,31 @@ 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;
@@ -1460,12 +1492,14 @@ void MsckfVio::twodotMeasurementJacobian(
   return;
 }
 
-void MsckfVio::twodotFeatureJacobian(
+bool 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];
   
 
@@ -1486,7 +1520,9 @@ void 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;
 
@@ -1494,6 +1530,7 @@ void 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);
@@ -1504,10 +1541,14 @@ void 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.
 
@@ -1555,16 +1596,16 @@ void MsckfVio::twodotFeatureJacobian(
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
-  return;
+  return true;
 }
 
 
 bool MsckfVio::PhotometricPatchPointResidual(
   const StateIDType& cam_state_id,
   const Feature& feature, 
-  VectorXd& r, int patchsize)
+  VectorXd& r)
 {
-  VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
+  VectorXd r_photo = VectorXd::Zero(2*N*N);
   int count = 0;
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
 
@@ -1575,19 +1616,18 @@ 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));
@@ -1598,31 +1638,14 @@ 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<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));
+  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));
     }
   }
- 
-  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)
   {
 
@@ -1648,7 +1671,6 @@ bool MsckfVio::PhotometricPatchPointResidual(
     count++;
   }
   r = r_photo;
-
   return true;
 }
 
@@ -1659,7 +1681,6 @@ 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,
@@ -1718,8 +1739,6 @@ 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}
@@ -1744,11 +1763,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[(patchsize*patchsize-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho*rho), 1/(rho*rho));
+  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_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
-                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho), 
-                                                 feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho), 
+                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), 
+                                                 feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), 
                                                  1/(rho)));
   dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
@@ -1771,61 +1790,41 @@ bool MsckfVio::PhotometricPatchPointJacobian(
 bool MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r, int patchsize) 
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
 {
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
   //photometric observation
-  VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
+  VectorXd r_photo;
 
-  // one line of the patchsizexpatchsize Jacobians
+  // one line of the NxN 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* patchsize*patchsize, 4);
+  Eigen::MatrixXd dI_dh(2* N*N, 4);
 
   // combined Jacobians
-  Eigen::MatrixXd H_rho(2 * patchsize*patchsize, 1);
-  Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6);
-  Eigen::MatrixXd H_pA(2 * patchsize*patchsize, 6);
+  Eigen::MatrixXd H_rho(2 * N*N, 1);
+  Eigen::MatrixXd H_pl(2 * N*N, 6);
+  Eigen::MatrixXd H_pA(2 * N*N, 6);
 
   // calcualte residual of patch
-  if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo, patchsize))
-  {
+  if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
     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;
-
-  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)
+  for (auto point : feature.anchorPatch_3d)
   {
     // get jacobi of single point in patch
-    if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, patchsize, H_rhoj, H_plj, H_pAj, dI_dhj))
+    if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
     {
         valid_count++;
         valid = true;
@@ -1837,6 +1836,7 @@ 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, patchsize);
+  ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
 
   // set to return values
   H_x = H_xl;
@@ -1868,8 +1868,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.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss);
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
@@ -1886,17 +1885,16 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
                                   const Feature& feature,
                                   const StateIDType&  cam_state_id,
                                   Eigen::MatrixXd& H_xl,
-                                  Eigen::MatrixXd& H_yl,
-                                  int patchsize)
+                                  Eigen::MatrixXd& H_yl)
 {
-  H_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7);
-  H_yl = MatrixXd::Zero(2*patchsize*patchsize, 1);
+  H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
+  H_yl = MatrixXd::Zero(2*N*N, 1);
 
   // get position of anchor in cam states
   auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
   int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
   // set anchor Jakobi
-  H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*patchsize*patchsize, 6) = H_pA; 
+  H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; 
 
   //get position of current frame in cam states
   auto cam_state_iter = state_server.cam_states.find(cam_state_id);
@@ -1904,7 +1902,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*patchsize*patchsize, 6) = H_pl;
+  H_xl.block(0, 21+cam_state_cntr*7, 2*N*N, 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);
@@ -1924,9 +1922,10 @@ 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,
-    int patchsize)
+    MatrixXd& H_x, VectorXd& r) 
 {
+  if(FILTER != 1)
+    return false;
 
   const auto& feature = map_server[feature_id];
 
@@ -1960,8 +1959,10 @@ 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, patchsize))
-          continue;
+    if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
+    {
+      continue;
+    }
 
     // set size of stacking jacobians, once the returned jacobians are known
     if(first)
@@ -1982,7 +1983,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;
@@ -2137,12 +2138,15 @@ void MsckfVio::measurementJacobian(
   return;
 }
 
-void MsckfVio::featureJacobian(
+bool 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];
 
@@ -2201,16 +2205,42 @@ void MsckfVio::featureJacobian(
   FullPivLU<MatrixXd> lu(H_fj.transpose());
   MatrixXd A = lu.kernel();
 
-   H_x = A.transpose() * H_xj;
-   r = A.transpose() * r_j;
+  H_x = A.transpose() * H_xj;
+  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/MSCKF_ws/log.txt");
-    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
+    myfile.open("/home/raphael/dev/octave/org2octave");
+    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+           << "# 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");
@@ -2249,7 +2279,7 @@ void MsckfVio::featureJacobian(
     myfile.close();
   }
 
-  return;
+  return true;
 
 }
 
@@ -2386,8 +2416,10 @@ 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;
@@ -2633,43 +2665,12 @@ 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]) {
-    if(filter == 1)
+    // cout << "passed" << endl;
     cout << "gate: " << dof << " " << gamma << " " <<
       chi_squared_test_table[dof] << endl;
-    // cout << "passed" << endl;
     return true;
   } else {
     // cout << "failed" << endl;
@@ -2682,7 +2683,6 @@ 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,7 +2724,6 @@ 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;
 
@@ -2753,10 +2752,6 @@ 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());
@@ -2773,17 +2768,12 @@ void MsckfVio::removeLostFeatures() {
 
     MatrixXd H_xj;
     VectorXd r_j;
-    
-    MatrixXd twoH_xj;
-    VectorXd twor_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))
+    MatrixXd twoH_xj;
+    VectorXd twor_j;
+    
+    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
@@ -2792,31 +2782,25 @@ void MsckfVio::removeLostFeatures() {
       }
     }
     
-    /*
-    cout << "3: " << endl;
-    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
+    if(featureJacobian(feature.id, cam_state_ids, H_xj, r_j))
     {
-        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);
-
-    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;
-      r.segment(stack_cntr, r_j.rows()) = r_j;
-      stack_cntr += H_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();
+      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;
+        r.segment(stack_cntr, r_j.rows()) = r_j;
+        stack_cntr += H_xj.rows();
+      }  
     }
+
+    if(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j))
+    {
+      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.
@@ -2829,16 +2813,22 @@ 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)
+  {
+    H_x.conservativeResize(stack_cntr, H_x.cols());
+    r.conservativeResize(stack_cntr);
+    measurementUpdate(H_x, r);
  
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  twor.conservativeResize(twostack_cntr);
-  
-  // Perform the measurement update step.
-  measurementUpdate(H_x, r);
-  twoMeasurementUpdate(twoH_x, twor);
-  
+  }
+  if(twostack_cntr)
+  {  
+    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+    twor.conservativeResize(twostack_cntr);
+    twoMeasurementUpdate(twoH_x, twor);
+  }
+ 
+
   // Remove all processed features from the map.
   for (const auto& feature_id : processed_feature_ids)
     map_server.erase(feature_id);
@@ -2901,7 +2891,6 @@ 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;
 
   
@@ -2940,7 +2929,6 @@ 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();
     
@@ -2956,16 +2944,11 @@ 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) {
@@ -2981,40 +2964,34 @@ 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;
         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);
-
-      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;
-      r.segment(stack_cntr, r_j.rows()) = r_j;
-      stack_cntr += H_xj.rows();
-      pruned_cntr++;
-      }
     
-    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();
+    if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_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;
+        r.segment(stack_cntr, r_j.rows()) = r_j;
+        stack_cntr += H_xj.rows();
+        pruned_cntr++;
+      }
+    }
+
+    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())) {
+        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)
@@ -3028,16 +3005,20 @@ void MsckfVio::pruneLastCamStateBuffer()
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  H_x.conservativeResize(stack_cntr, H_x.cols());
-  r.conservativeResize(stack_cntr);
- 
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  twor.conservativeResize(twostack_cntr);
- 
-  // Perform the measurement update step.
-  measurementUpdate(H_x, r);
-  twoMeasurementUpdate(twoH_x, twor);
+  if(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);
+    twoMeasurementUpdate(twoH_x, twor);
+  }
+
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
       state_server.cam_states.find(rm_cam_state_id));
@@ -3089,7 +3070,6 @@ 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) {
@@ -3134,10 +3114,9 @@ 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;
   }
 
@@ -3149,19 +3128,11 @@ 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());
@@ -3180,43 +3151,31 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
 
-    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)) {// 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(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
     {
-      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();
-      }    
+      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();
+      }
     }
-    */
-
-    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-    twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
-
-    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(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())) {
+        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();
+      }
     }
 
-    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);
 
@@ -3229,16 +3188,20 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  H_x.conservativeResize(stack_cntr, H_x.cols());
-  r.conservativeResize(stack_cntr);
- 
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  twor.conservativeResize(twostack_cntr);
-  
-  // Perform the measurement update step.
-  measurementUpdate(H_x, r);
-  twoMeasurementUpdate(twoH_x, twor);
-  
+  if(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);
+    twoMeasurementUpdate(twoH_x, twor);
+  }
+
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),