added two filter, not working yet - compare with htest

include/msckf_vio/msckf_vio.h

@@ -207,6 +207,11 @@ class MsckfVio {
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
 
+    void twodotMeasurementJacobian(
+    const StateIDType& cam_state_id,
+    const FeatureIDType& feature_id,
+    Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
+
     void PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
@@ -214,6 +219,11 @@ class MsckfVio {
     Eigen::MatrixXd& H_y,
     Eigen::VectorXd& r);
 
+    void twodotFeatureJacobian(
+    const FeatureIDType& feature_id,
+    const std::vector<StateIDType>& cam_state_ids,
+    Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
     void PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
@@ -222,6 +232,8 @@ class MsckfVio {
     void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
     void measurementUpdate(const Eigen::MatrixXd& H,
         const Eigen::VectorXd& r);
+    void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
+
     bool gatingTest(const Eigen::MatrixXd& H,
         const Eigen::VectorXd&r, const int& dof);
     void removeLostFeatures();
@@ -234,7 +246,7 @@ class MsckfVio {
     void onlineReset();
 
     // Photometry flag
-    bool PHOTOMETRIC;
+    int FILTER;
 
     // debug flag
     bool STREAMPAUSE;

launch/msckf_vio_euroc.launch

@@ -17,9 +17,8 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <!-- Photometry Flag-->
+      <param name="FILTER" value="2"/>
-      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>

launch/msckf_vio_tinytum.launch

@@ -17,8 +17,8 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-      <!-- Photometry Flag-->
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="PHOTOMETRIC" value="false"/>
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>

src/msckf_vio.cpp

@@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
 
 bool MsckfVio::loadParameters() {
   //Photometry Flag
-  nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
+  nh.param<int>("FILTER", FILTER, 0);
   nh.param<bool>("PrintImages", PRINTIMAGES, false);
   nh.param<bool>("StreamPause", STREAMPAUSE, false);
   nh.param<bool>("GroundTruth", GROUNDTRUTH, false);
@@ -405,8 +405,7 @@ void MsckfVio::imageCallback(
     const sensor_msgs::ImageConstPtr& cam1_img,
     const CameraMeasurementConstPtr& feature_msg)
 {
-  cout << "hello" << endl;
+   // stop playing bagfile if printing images
-      // stop playing bagfile if printing images
   if(STREAMPAUSE)
     nh.setParam("/play_bag", false);
   // Return if the gravity vector has not been set.
@@ -455,7 +454,7 @@ void MsckfVio::imageCallback(
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "1" << endl;
+  cout << "1" << endl;
   // Augment the state vector.
   start_time = ros::Time::now();
   //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@@ -464,7 +463,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "2" << endl;
+  cout << "2" << endl;
   // Add new observations for existing features or new
   // features in the map server.
   start_time = ros::Time::now();
@@ -473,7 +472,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "3" << endl;
+  cout << "3" << endl;
   // Add new images to moving window
   start_time = ros::Time::now();
   manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -481,20 +480,20 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "4" << endl;
+  cout << "4" << endl;
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
   double remove_lost_features_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "5" << endl;
+  cout << "5" << endl;
   start_time = ros::Time::now();
   pruneLastCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "6" << endl;
+  cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
   publish(feature_msg->header.stamp);
@@ -1243,6 +1242,223 @@ void MsckfVio::addFeatureObservations(
   return;
 }
 
+void MsckfVio::twodotMeasurementJacobian(
+    const StateIDType& cam_state_id,
+    const FeatureIDType& feature_id,
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
+{
+
+  // Prepare all the required data.
+  const CAMState& cam_state = state_server.cam_states[cam_state_id];
+  const Feature& feature = map_server[feature_id];
+
+  // Cam0 pose.
+  Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
+  const Vector3d& t_c0_w = cam_state.position;
+
+  //photometric observation
+  std::vector<double> photo_z;
+
+  // individual Jacobians
+  Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
+  Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
+  Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
+  Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
+  Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
+
+  Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
+
+  // one line of the NxN Jacobians
+  Eigen::Matrix<double, 2, 1> H_rho;
+  Eigen::Matrix<double, 2, 6> H_plj;
+  Eigen::Matrix<double, 2, 6> H_pAj;
+
+  auto frame = cam0.moving_window.find(cam_state_id)->second.image;
+
+  int count = 0;
+
+  auto point = feature.anchorPatch_3d[0];
+
+  Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
+  // add jacobian
+  
+  //dh / d{}^Cp_{ij}
+  dh_dCpij(0, 0) = 1 / p_c0(2);
+  dh_dCpij(1, 1) = 1 / p_c0(2);
+  dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
+  dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
+
+  dCpij_dGpij = quaternionToRotation(cam_state.orientation);
+
+  //orientation takes camera frame to world frame
+  dh_dGpij = dh_dCpij * dCpij_dGpij;
+
+  //dh / d X_{pl}
+  dCpij_dCGtheta = skewSymmetric(p_c0);
+  dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
+  dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
+  dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
+
+  //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[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
+
+
+
+  // alternative derivation towards feature
+  Matrix3d dCpc0_dpg = R_w_c0;
+  dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
+                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
+                                                 feature.anchorPatch_ideal[count].y/(rho), 
+                                                 1/(rho)));
+  dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
+
+  // Intermediate Jakobians
+  H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1
+  H_plj = dh_dXplj; // 2 x 6
+  H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6
+
+  // calculate residual
+
+  //observation
+  const Vector4d& total_z = feature.observations.find(cam_state_id)->second;
+  const Vector2d z = Vector2d(total_z[0], total_z[1]);
+ 
+  VectorXd r_i = VectorXd::Zero(2);
+  
+  //calculate residual
+
+  r_i[0] = z[0] - p_c0(0)/p_c0(2);
+  r_i[1] = z[1] - p_c0(1)/p_c0(2);
+
+  MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
+ 
+  // set anchor Jakobi
+    // 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);
+  H_xl.block(0, 21+cam_state_cntr_anchor*7, 2, 6) = H_pAj; 
+
+  // set frame Jakobi
+    //get position of current frame in cam states
+  auto cam_state_iter = state_server.cam_states.find(cam_state_id);
+  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, 6) = H_plj;
+
+  H_x = H_xl;
+  H_y = H_rho;
+  r = r_i;
+
+  //TODO make this more fluent as well
+  if(PRINTIMAGES)
+  {  
+    std::stringstream ss;
+    ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
+    feature.MarkerGeneration(marker_pub, state_server.cam_states);
+    //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
+  }
+
+  return;
+}
+
+void MsckfVio::twodotFeatureJacobian(
+    const FeatureIDType& feature_id,
+    const std::vector<StateIDType>& cam_state_ids,
+    MatrixXd& H_x, VectorXd& r) 
+{
+
+  const auto& feature = map_server[feature_id];
+  
+
+  // Check how many camera states in the provided camera
+  // id camera has actually seen this feature.
+  vector<StateIDType> valid_cam_state_ids(0);
+  for (const auto& cam_id : cam_state_ids) {
+    if (feature.observations.find(cam_id) ==
+        feature.observations.end()) continue;
+
+    if (feature.observations.find(cam_id) ==
+        feature.observations.begin()) continue;
+    valid_cam_state_ids.push_back(cam_id);
+  }
+
+  int jacobian_row_size = 0;
+  jacobian_row_size = 2 * valid_cam_state_ids.size();
+
+  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;
+
+  for (const auto& cam_id : valid_cam_state_ids) {
+
+    MatrixXd H_xl;
+    MatrixXd H_yl;
+    Eigen::VectorXd r_l = VectorXd::Zero(2);
+
+    twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
+    auto cam_state_iter = state_server.cam_states.find(cam_id);
+    int cam_state_cntr = std::distance(
+        state_server.cam_states.begin(), cam_state_iter);
+
+    // Stack the Jacobians.
+    H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
+    H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
+    r_i.segment(stack_cntr, 2) = r_l;
+    stack_cntr += 2;
+  }
+
+  // Project the residual and Jacobians onto the nullspace
+  // of H_yj.
+
+  // get Nullspace
+  FullPivLU<MatrixXd> lu(H_yi.transpose());
+  MatrixXd A_null_space = lu.kernel();
+
+  H_x = A_null_space.transpose() * H_xi;
+  r = A_null_space.transpose() * r_i;
+
+  if(PRINTIMAGES)
+  {
+
+  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: Hx\n"
+         << "# type: matrix\n"
+         << "# rows: " << H_xi.rows() << "\n"
+         << "# columns: " << H_xi.cols() << "\n"
+         << H_xi << endl;
+
+  myfile << "# name: Hy\n"
+         << "# type: matrix\n"
+         << "# rows: " << H_yi.rows() << "\n"  
+         << "# columns: " << H_yi.cols() << "\n"
+         << H_yi << endl;
+
+
+  myfile << "# name: r\n"
+         << "# type: matrix\n"
+         << "# rows: " << r_i.rows() << "\n"  
+         << "# columns: " << 1 << "\n"
+         << r_i << endl;
+
+  myfile.close();
+    std::cout << "resume playback" << std::endl;
+    nh.setParam("/play_bag", true);
+  }
+  return;
+}
+
+
+
 void MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
@@ -1431,6 +1647,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   return;
 }
 
+
 void MsckfVio::PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
@@ -1737,7 +1954,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
 
-  if(PHOTOMETRIC) return;
+  if(FILTER != 0) return;
   
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -1793,6 +2010,108 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   return;
 }
 
+void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
+
+   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;
+  VectorXd r_thin;
+  
+  // QR decomposition to make stuff faster
+  if (H.rows() > H.cols()) {
+    // Convert H to a sparse matrix.
+    SparseMatrix<double> H_sparse = H.sparseView();
+
+    // Perform QR decompostion on H_sparse.
+    SPQR<SparseMatrix<double> > spqr_helper;
+    spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
+    spqr_helper.compute(H_sparse);
+
+    MatrixXd H_temp;
+    VectorXd r_temp;
+    (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
+    (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
+
+    H_thin = H_temp.topRows(21+state_server.cam_states.size()*7);
+    r_thin = r_temp.head(21+state_server.cam_states.size()*7);
+
+  } else {
+    H_thin = H;
+    r_thin = 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 << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
+  cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+  // Update the IMU state.
+  if (FILTER != 2) return;
+
+  const VectorXd& delta_x_imu = delta_x.head<21>();
+
+  if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
+      //delta_x_imu.segment<3>(3).norm() > 0.15 ||
+      delta_x_imu.segment<3>(6).norm() > 0.5 ||
+      //delta_x_imu.segment<3>(9).norm() > 0.5 ||
+      delta_x_imu.segment<3>(12).norm() > 1.0) {
+    printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm());
+    printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm());
+    ROS_WARN("Update change is too large.");
+    //return;
+  }
+
+  const Vector4d dq_imu =
+    smallAngleQuaternion(delta_x_imu.head<3>());
+  state_server.imu_state.orientation = quaternionMultiplication(
+      dq_imu, state_server.imu_state.orientation);
+  state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3);
+  state_server.imu_state.velocity += delta_x_imu.segment<3>(6);
+  state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9);
+  state_server.imu_state.position += delta_x_imu.segment<3>(12);
+
+  const Vector4d dq_extrinsic =
+    smallAngleQuaternion(delta_x_imu.segment<3>(15));
+  state_server.imu_state.R_imu_cam0 = quaternionToRotation(
+      dq_extrinsic) * state_server.imu_state.R_imu_cam0;
+  state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18);
+
+  // Update the camera states.
+  auto cam_state_iter = state_server.cam_states.begin();
+  for (int i = 0; i < state_server.cam_states.size();
+      ++i, ++cam_state_iter) {
+    const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6);
+    const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
+    cam_state_iter->second.orientation = quaternionMultiplication(
+        dq_cam, cam_state_iter->second.orientation);
+    cam_state_iter->second.position += delta_x_cam.tail<3>();
+    cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6);
+  }
+
+  // Update state covariance.
+  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
+  //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
+  //  K*K.transpose()*Feature::observation_noise;
+  state_server.state_cov = I_KH*state_server.state_cov;
+
+  // Fix the covariance to be symmetric
+  MatrixXd state_cov_fixed = (state_server.state_cov +
+      state_server.state_cov.transpose()) / 2.0;
+  state_server.state_cov = state_cov_fixed;
+  return;
+}
+
 
 void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
@@ -1841,7 +2160,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
   cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
   cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
-  if (not PHOTOMETRIC) return;
+  if (FILTER != 1) return;
 
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -1925,6 +2244,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 twojacobian_row_size = 0;
 
   vector<FeatureIDType> invalid_feature_ids(0);
   vector<FeatureIDType> processed_feature_ids(0);
@@ -1965,7 +2285,9 @@ void MsckfVio::removeLostFeatures() {
     }
 
     pjacobian_row_size += N*N*feature.observations.size();
+    twojacobian_row_size += 2*feature.observations.size();
     jacobian_row_size += 4*feature.observations.size() - 3;
+
     processed_feature_ids.push_back(feature.id);
   }
 
@@ -1974,6 +2296,9 @@ void MsckfVio::removeLostFeatures() {
   //  processed_feature_ids.size() << endl;
   //cout << "jacobian row #: " << jacobian_row_size << endl;
 
+
+  cout << "sizing" << endl;
+
   // Remove the features that do not have enough measurements.
   for (const auto& feature_id : invalid_feature_ids)
     map_server.erase(feature_id);
@@ -1991,6 +2316,12 @@ void MsckfVio::removeLostFeatures() {
   VectorXd pr = VectorXd::Zero(pjacobian_row_size);
   int pstack_cntr = 0;
 
+
+  MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size,
+      21+7*state_server.cam_states.size());
+  VectorXd twor = VectorXd::Zero(twojacobian_row_size);
+  int twostack_cntr = 0;
+
   // Process the features which lose track.
   for (const auto& feature_id : processed_feature_ids) {
     auto& feature = map_server[feature_id];
@@ -2003,10 +2334,18 @@ void MsckfVio::removeLostFeatures() {
     VectorXd r_j;
     MatrixXd pH_xj;
     VectorXd pr_j;
+    MatrixXd twoH_xj;
+    VectorXd twor_j;
+
+
+    cout << "measuring" << endl;
 
     PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
-    
+    twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
+
+    cout << "gating" << endl;
+
     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;
@@ -2017,20 +2356,29 @@ void MsckfVio::removeLostFeatures() {
       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;
+      twostack_cntr += twoH_xj.rows();
+    }
 
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
     //if (stack_cntr > 1500) break;
   }
 
+  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.
   for (const auto& feature_id : processed_feature_ids)
@@ -2094,6 +2442,7 @@ void MsckfVio::pruneLastCamStateBuffer()
   // Set the size of the Jacobian matrix.
   int jacobian_row_size = 0;
   int pjacobian_row_size = 0;
+  int twojacobian_row_size = 0;
 
   
   //initialize all the features which are going to be removed
@@ -2132,6 +2481,7 @@ void MsckfVio::pruneLastCamStateBuffer()
 
     pjacobian_row_size += N*N*feature.observations.size();
     jacobian_row_size += 4*feature.observations.size() - 3;
+    twojacobian_row_size += 2*feature.observations.size();
     
   }
 
@@ -2143,9 +2493,14 @@ void MsckfVio::pruneLastCamStateBuffer()
   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);
+  MatrixXd twoH_xj;
+  VectorXd twor_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 twostack_cntr = 0;
 
   for (auto& item : map_server) {
     auto& feature = item.second;
@@ -2162,6 +2517,7 @@ void MsckfVio::pruneLastCamStateBuffer()
 
       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, r_j.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@@ -2174,6 +2530,11 @@ void MsckfVio::pruneLastCamStateBuffer()
       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;
+      twostack_cntr += twoH_xj.rows();
+    }
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -2184,12 +2545,16 @@ void MsckfVio::pruneLastCamStateBuffer()
   
   pH_x.conservativeResize(pstack_cntr, pH_x.cols());
   pr.conservativeResize(pstack_cntr);
+  // Perform measurement update.  
+
+  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
   int cam_sequence = std::distance(state_server.cam_states.begin(),
       state_server.cam_states.find(rm_cam_state_id));
@@ -2241,6 +2606,7 @@ void MsckfVio::pruneCamStateBuffer() {
   // Find the size of the Jacobian matrix.
   int jacobian_row_size = 0;
   int pjacobian_row_size = 0;
+  int twojacobian_row_size = 0;
 
   for (auto& item : map_server) {
     auto& feature = item.second;
@@ -2285,6 +2651,7 @@ void MsckfVio::pruneCamStateBuffer() {
       }
     }
     
+    twojacobian_row_size += 2*involved_cam_state_ids.size();
     pjacobian_row_size += N*N*involved_cam_state_ids.size();
     jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
   }
@@ -2302,6 +2669,11 @@ void MsckfVio::pruneCamStateBuffer() {
   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 twoH_xj;
+  VectorXd twor_j;
+  MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
+  VectorXd twor = VectorXd::Zero(twojacobian_row_size);
+  int twostack_cntr = 0;
   for (auto& item : map_server) {
     auto& feature = item.second;
     // Check how many camera states to be removed are associated
@@ -2317,6 +2689,7 @@ void MsckfVio::pruneCamStateBuffer() {
     
     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;
@@ -2329,6 +2702,11 @@ void MsckfVio::pruneCamStateBuffer() {
       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;
+      twostack_cntr += twoH_xj.rows();
+    }
 
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
@@ -2339,11 +2717,13 @@ void MsckfVio::pruneCamStateBuffer() {
   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) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),