added multiple couts for testing, not working

include/msckf_vio/feature.hpp

@@ -1024,8 +1024,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
   cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
 
-  xderImage/=8;
+  xderImage/=8.;
-  yderImage/=8;
+  yderImage/=8.;
 
   cv::convertScaleAbs(xderImage, abs_xderImage);
   cv::convertScaleAbs(yderImage, abs_yderImage);

include/msckf_vio/msckf_vio.h

@@ -212,7 +212,7 @@ class MsckfVio {
     const FeatureIDType& feature_id,
     Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
 
-    void PhotometricMeasurementJacobian(
+    bool PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
     Eigen::MatrixXd& H_x,
@@ -224,7 +224,7 @@ class MsckfVio {
     const std::vector<StateIDType>& cam_state_ids,
     Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
-    void PhotometricFeatureJacobian(
+    bool PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -263,6 +263,11 @@ class MsckfVio {
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
+
+    // change in position
+    Eigen::Vector3d delta_position;
+    Eigen::Vector3d delta_orientation;
+
     // State vector
     StateServer state_server;
     StateServer photometric_state_server;

launch/msckf_vio_tum.launch

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

src/msckf_vio.cpp

@@ -412,8 +412,6 @@ void MsckfVio::imageCallback(
     return;
   }
 
-
-  
   // Start the system if the first image is received.
   // The frame where the first image is received will be
   // the origin.
@@ -433,10 +431,6 @@ void MsckfVio::imageCallback(
   
   batchImuProcessing(feature_msg->header.stamp.toSec());
   
-  // save true state in seperate state vector
-
-  batchTruthProcessing(feature_msg->header.stamp.toSec());
-  
   if(GROUNDTRUTH)
   {
     state_server.imu_state.position = true_state_server.imu_state.position;
@@ -489,6 +483,10 @@ void MsckfVio::imageCallback(
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
+  // save true state in seperate state vector and calculate relative error in change
+  batchTruthProcessing(feature_msg->header.stamp.toSec());
+
+
   cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
@@ -808,6 +806,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   // Counter how many IMU msgs in the buffer are used.
   int used_truth_msg_cntr = 0;
 
+  const IMUState old_true_state = true_state_server.imu_state;
+
   for (const auto& truth_msg : truth_msg_buffer) {
     double truth_time = truth_msg.header.stamp.toSec();
     if (truth_time < true_state_server.imu_state.time) {
@@ -839,6 +839,22 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
       truth_msg_buffer.begin()+used_truth_msg_cntr);
 
+  // calculate error in position
+
+  // calculate error in change
+
+  // calculate change
+  Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position;
+  Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation);
+  Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2);
+  Eigen::Vector3d error_delta_position = delta_true_position - delta_position;
+  Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation  - delta_orientation;
+  
+  double dx  = (error_delta_position[0]/delta_true_position[0]);
+  double dy = (error_delta_position[1]/delta_true_position[1]);
+  double dz = (error_delta_position[2]/delta_true_position[2]);
+  cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl;
+
 }
 
 void MsckfVio::processTruthtoIMU(const double& time,
@@ -1463,7 +1479,7 @@ void MsckfVio::twodotFeatureJacobian(
 
 
 
-void MsckfVio::PhotometricMeasurementJacobian(
+bool MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
     MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
@@ -1531,6 +1547,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   int count = 0;
   double dx, dy;
 
+  cout << "patching" << endl;
   for (auto point : feature.anchorPatch_3d)
   {
     //cout << "____feature-measurement_____\n" << endl;
@@ -1538,6 +1555,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
     cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
 
+    if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1)
+    {
+      cout << "skipped" << endl;
+      return false;
+    }
     //add observation
     photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
 
@@ -1597,8 +1619,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
     count++;
   }
 
+  cout << "done" << endl;
   MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
-  MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
+  MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
 
   // set anchor Jakobi
     // get position of anchor in cam states
@@ -1616,15 +1639,18 @@ void MsckfVio::PhotometricMeasurementJacobian(
   H_xl.block(0, 21+cam_state_cntr*7, 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);
+  // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
 
   // set irradiance error Block
-  H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
+  H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
   
   // TODO make this calculation more fluent
-  for(int i = 0; i< N*N; i++)   
+  
+  /*for(int i = 0; i< N*N; i++)   
     H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
-  H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
+  */
+
+  H_yl.block(0, N*N, N*N, 1) = -H_rho;
 
   H_x = H_xl;
   H_y = H_yl;
@@ -1636,15 +1662,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
   if(PRINTIMAGES)
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
-  return;
+  cout << "returning" << endl;
+  return true;
 }
 
 
-void MsckfVio::PhotometricFeatureJacobian(
+bool MsckfVio::PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
@@ -1669,7 +1696,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
-  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1679,7 +1706,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     MatrixXd H_yl;
     Eigen::VectorXd r_l = VectorXd::Zero(N*N);
 
-    PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
+    if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
+      continue;
 
     auto cam_state_iter = state_server.cam_states.find(cam_id);
     int cam_state_cntr = std::distance(
@@ -1691,7 +1719,11 @@ void MsckfVio::PhotometricFeatureJacobian(
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
   }
-
+  if(stack_cntr == 0)
+  {
+    cout << "skip feature" << endl;
+    return false;
+  }
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
@@ -1754,12 +1786,20 @@ void MsckfVio::PhotometricFeatureJacobian(
            << "# columns: " << A_null_space.cols() << "\n"
            << A_null_space << endl;
 
+    myfile << "# name: P\n"
+           << "# type: matrix\n"
+           << "# rows: " << state_server.state_cov.rows() << "\n"  
+           << "# columns: " <<  state_server.state_cov.rows() << "\n"
+           <<  state_server.state_cov << endl;
+
+
     myfile.close();
     cout << "---------- LOGGED -------- " << endl; 
-    cvWaitKey(0);
   }
 
-  return;
+  cout << "donefeature" << endl;
+
+  return true;
 }
 
 void MsckfVio::measurementJacobian(
@@ -1929,6 +1969,13 @@ void MsckfVio::featureJacobian(
            << "# columns: " << A.cols() << "\n"
            << A << endl;
 
+    myfile << "# name: P\n"
+           << "# type: matrix\n"
+           << "# rows: " << state_server.state_cov.rows() << "\n"  
+           << "# columns: " <<  state_server.state_cov.rows() << "\n"
+           <<  state_server.state_cov << endl;
+
+
     myfile.close();
   }
 
@@ -1970,19 +2017,50 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;
+  
   MatrixXd S = H_thin*P*H_thin.transpose() +
       Feature::observation_noise*MatrixXd::Identity(
         H_thin.rows(), H_thin.rows());
+  
+
+
   //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
   MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
   MatrixXd K = K_transpose.transpose();
   // Compute the error of the state.
   VectorXd delta_x = K * r;
+
+
   cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
   cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
-  // Update the IMU state.
 
   if(FILTER != 0) return;
+
+  if(PRINTIMAGES)
+  {
+    //octave
+    ofstream myfile;
+    
+    myfile.open("/home/raphael/dev/octave/measurement2octave");
+    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+           << "# name: K\n"
+           << "# type: matrix\n"
+           << "# rows: " << K.rows() << "\n"
+           << "# columns: " << K.cols() << "\n"
+           << K << endl;
+
+    myfile << "# name: r\n"
+           << "# type: matrix\n"
+           << "# rows: " << r.rows() << "\n"
+           << "# columns: " << r.cols() << "\n"
+           << r << endl;
+
+    myfile.close();
+  }
+  delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
+  delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
+
+  // Update the IMU state.
   
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -2046,7 +2124,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
   VectorXd r_thin;
-/*
+
   if (H.rows() > H.cols()) {
     // Convert H to a sparse matrix.
     SparseMatrix<double> H_sparse = H.sparseView();
@@ -2070,10 +2148,10 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
     //H_thin = Q1.transpose() * H;
     //r_thin = Q1.transpose() * r;
-  } else {*/
+  } else {
     H_thin = H;
     r_thin = r;
-  //}
+  }
 
 
 
@@ -2093,6 +2171,9 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
   // Update the IMU state.
   if (FILTER != 2) return;
 
+  delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
+  delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
+
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
   if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
@@ -2196,6 +2277,33 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
   // Update the IMU state.
   if (FILTER != 1) return;
 
+  cout << "here" << endl;
+
+  if(PRINTIMAGES)
+  {
+  //octave
+  ofstream myfile;
+  
+  myfile.open("/home/raphael/dev/octave/measurement2octave");
+  myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+         << "# name: K\n"
+         << "# type: matrix\n"
+         << "# rows: " << K.rows() << "\n"
+         << "# columns: " << K.cols() << "\n"
+         << K << endl;
+
+  myfile << "# name: r\n"
+         << "# type: matrix\n"
+         << "# rows: " << r.rows() << "\n"
+         << "# columns: " << r.cols() << "\n"
+         << r << endl;
+
+  myfile.close();
+  }
+
+  delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
+  delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
+
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
   if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
@@ -2374,8 +2482,18 @@ void MsckfVio::removeLostFeatures() {
 
     cout << "measuring" << endl;
 
-    PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
+    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true);
-    cout << "norm" << endl;
+    {
+            cout << "p gating" << endl;
+
+      if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, 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 << "donephoto" << endl;
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     cout << "two" << endl;
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@@ -2387,11 +2505,6 @@ void MsckfVio::removeLostFeatures() {
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
     }  
-    if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, 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();
-    }
     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;
@@ -2403,17 +2516,23 @@ void MsckfVio::removeLostFeatures() {
     //if (stack_cntr > 1500) break;
   }
 
+  if(pstack_cntr)
+  {
+    pH_x.conservativeResize(pstack_cntr, pH_x.cols());
+    pr.conservativeResize(pstack_cntr);
+  
+    photometricMeasurementUpdate(pH_x, pr);
+  }
+
   cout << "resizing" << endl;
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
-  pH_x.conservativeResize(pstack_cntr, pH_x.cols());
+ 
-  pr.conservativeResize(pstack_cntr);
   twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
   twor.conservativeResize(twostack_cntr);
   
   // Perform the measurement update step.
   measurementUpdate(H_x, r);
-  photometricMeasurementUpdate(pH_x, pr);
   twoMeasurementUpdate(twoH_x, twor);
 
   // Remove all processed features from the map.
@@ -2553,7 +2672,18 @@ void MsckfVio::pruneLastCamStateBuffer()
 
     cout << "measuring" << endl;
 
-    PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
+    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true);
+    {
+            cout << "p gating" << endl;
+
+      if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, 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 << "norm" << endl;
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     cout << "two" << endl;
@@ -2566,11 +2696,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       stack_cntr += H_xj.rows();
       pruned_cntr++;
       }
-    if (gatingTest(pH_xj, pr_j, pr_j.size())) {// 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();
-    }
     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;
@@ -2580,12 +2706,17 @@ void MsckfVio::pruneLastCamStateBuffer()
       feature.observations.erase(cam_id);
   }
 
+  if (pstack_cntr)
+  {
+    pH_x.conservativeResize(pstack_cntr, pH_x.cols());
+    pr.conservativeResize(pstack_cntr);
+
+    photometricMeasurementUpdate(pH_x, pr);  
+  }
+
 
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
-  
-  pH_x.conservativeResize(pstack_cntr, pH_x.cols());
-  pr.conservativeResize(pstack_cntr);
   // Perform measurement update.  
 
   twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
@@ -2594,7 +2725,6 @@ void MsckfVio::pruneLastCamStateBuffer()
 
 
   measurementUpdate(H_x, r);
-  photometricMeasurementUpdate(pH_x, pr);
   twoMeasurementUpdate(twoH_x, twor);
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
@@ -2729,28 +2859,28 @@ void MsckfVio::pruneCamStateBuffer() {
     if (involved_cam_state_ids.size() == 0) continue;
         cout << "measuring" << endl;
 
-    PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
+    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
+    { 
+      cout << "p gating" << endl;
+      if (gatingTest(pH_xj, pr_j, pr_j.size())) {// 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();
+      }
+    }
     cout << "norm" << endl;
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     cout << "two" << endl;
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
     cout << "gating" << endl;
-    PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
-    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 (gatingTest(pH_xj, pr_j, pr_j.size())) {// 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();
-    }
     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;
@@ -2762,16 +2892,22 @@ void MsckfVio::pruneCamStateBuffer() {
   }
 
 
+  if(pstack_cntr > 0)
+  {
+    pH_x.conservativeResize(pstack_cntr, pH_x.cols());
+    pr.conservativeResize(pstack_cntr); 
+  
+    photometricMeasurementUpdate(pH_x, pr);
+  }
+
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
-  pH_x.conservativeResize(pstack_cntr, pH_x.cols());
+
-  pr.conservativeResize(pstack_cntr);
   twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
   twor.conservativeResize(twostack_cntr);
 
   // Perform measurement update.
   measurementUpdate(H_x, r);
-  photometricMeasurementUpdate(pH_x, pr);
   twoMeasurementUpdate(twoH_x, twor);
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {