added stereo camera residual and jacobi to twomsckf - works

include/msckf_vio/cam_state.h

@@ -18,6 +18,8 @@ namespace msckf_vio {
 
 struct Frame{
   cv::Mat image;
+  cv::Mat dximage;
+  cv::Mat dyimage;
   double exposureTime_ms;
 };
 

include/msckf_vio/feature.hpp

@@ -186,7 +186,8 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
 
   double CompleteCvKernel(
                 const cv::Point2f pose,
-                const cv::Mat& frame,
+                const StateIDType& cam_state_id,
+                CameraCalibration& cam,
                 std::string type) const;
 
   double cvKernel(
@@ -495,40 +496,17 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
 
 double Feature::CompleteCvKernel(
                 const cv::Point2f pose,
-                const cv::Mat& frame,
+                const StateIDType& cam_state_id,
+                CameraCalibration& cam,
                 std::string type) const
 {
 
   double delta = 0;
 
-  cv::Mat xder;
-  cv::Mat yder;
-  cv::Mat deeper_frame;
-  frame.convertTo(deeper_frame,CV_16S);
-  //TODO remove this?
-
-
-  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
-  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
-
-  xder/=8.;
-  yder/=8.;
-
-  /*
-  cv::Mat norm_abs_xderImage;
-  cv::Mat abs_xderImage2;
-  cv::convertScaleAbs(xder, abs_xderImage2);
-
-  cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
-
-  cv::imshow("xder", norm_abs_xderImage);
-  cvWaitKey(0);
-  */
-
   if(type == "Sobel_x")
-    delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
+    delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
   else if (type == "Sobel_y")
-    delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
+    delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
   return delta;
 }
 

launch/msckf_vio_tinytum.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
+      <param name="FILTER" value="2"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" 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="1"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -544,13 +544,22 @@ void MsckfVio::manageMovingWindow(
   image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
 
 
-
   // save image information into moving window
   cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
   cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
 
+  cv::Mat xder;
+  cv::Mat yder;
+  cv::Mat deeper_frame;
+  cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
 
+  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
+  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
+  xder/=8.;
+  yder/=8.;
 
+  cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); 
+  cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
 
 
 
@@ -1310,96 +1319,98 @@ void MsckfVio::twodotMeasurementJacobian(
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
-  // Cam0 pose.
+  // Cam0 pose
   Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Vector3d& t_c0_w = cam_state.position;
 
-  //photometric observation
+  // Cam1 pose
-  std::vector<double> photo_z;
+  Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
+  Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
+  Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
 
   // individual Jacobians
-  Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
+  Matrix<double, 4, 3> dh_dC0pij = Matrix<double, 4, 3>::Zero();
-  Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
+  Matrix<double, 4, 3> dh_dC1pij = Matrix<double, 4, 3>::Zero();
-  Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
+  Matrix<double, 4, 3> dh_dGpij = Matrix<double, 4, 3>::Zero();
+  Matrix<double, 4, 6> dh_dXplj = Matrix<double, 4, 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> dC0pij_dGpij = Matrix<double, 3, 3>::Zero();
-  Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 3> dC1pij_dGpij = Matrix<double, 3, 3>::Zero();
-  Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
+  Matrix<double, 3, 6> dC0pij_dXplj = Matrix<double, 3, 6>::Zero();
+  Matrix<double, 3, 6> dC1pij_dXplj = Matrix<double, 3, 6>::Zero();
 
   // one line of the NxN Jacobians
-  Eigen::Matrix<double, 2, 1> H_rho;
+  Eigen::Matrix<double, 4, 1> H_rho;
-  Eigen::Matrix<double, 2, 6> H_plj;
+  Eigen::Matrix<double, 4, 6> H_plj;
-  Eigen::Matrix<double, 2, 6> H_pAj;
+  Eigen::Matrix<double, 4, 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);
+  Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w);
   // add jacobian
   
   //dh / d{}^Cp_{ij}
-  dh_dCpij(0, 0) = 1 / p_c0(2);
+  dh_dC0pij(0, 0) = 1. / p_c0(2);
-  dh_dCpij(1, 1) = 1 / p_c0(2);
+  dh_dC0pij(1, 1) = 1. / p_c0(2);
-  dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
+  dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
-  dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
+  dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
 
-  dCpij_dGpij = quaternionToRotation(cam_state.orientation);
+  //dh / d{}^Cp_{ij}
+  dh_dC1pij(2, 0) = 1. / p_c1(2);
+  dh_dC1pij(3, 1) = 1. / p_c1(2);
+  dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2));
+  dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2));
 
-  //orientation takes camera frame to world frame
+  dC0pij_dGpij = R_w_c0;
-  dh_dGpij = dh_dCpij * dCpij_dGpij;
+  dC1pij_dGpij = R_c0_c1 * R_w_c0;
+
+  dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0);
+  dC0pij_dXplj.rightCols(3) = -R_w_c0;
 
-  //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));
+  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));
 
-
-
-  // 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), 
+                               * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), 
-                                                 feature.anchorPatch_ideal[count].y/(rho), 
+                                                 feature.anchorPatch_ideal[(N*N-1)/2].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_rho = dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1
-  H_plj = dh_dXplj; // 2 x 6
+  H_plj = dh_dC0pij * dC0pij_dXplj + dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6
-  H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6
+  H_pAj = dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 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);
+  VectorXd r_i = VectorXd::Zero(4);
   
   //calculate residual
 
-  r_i[0] = z[0] - p_c0(0)/p_c0(2);
+  r_i[0] = total_z[0] - p_c0(0)/p_c0(2);
-  r_i[1] = z[1] - p_c0(1)/p_c0(2);
+  r_i[1] = total_z[1] - p_c0(1)/p_c0(2);
+  r_i[2] = total_z[2] - p_c1(0)/p_c1(2);
+  r_i[3] = total_z[3] - p_c1(1)/p_c1(2);
   
-  MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
+  MatrixXd H_xl = MatrixXd::Zero(4, 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; 
+  H_xl.block(0, 21+cam_state_cntr_anchor*7, 4, 6) = H_pAj; 
 
   // set frame Jakobi
     //get position of current frame in cam states
@@ -1407,7 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian(
   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_xl.block(0, 21+cam_state_cntr*7, 4, 6) = H_plj;
 
   H_x = H_xl;
   H_y = H_rho;
@@ -1446,7 +1457,7 @@ void MsckfVio::twodotFeatureJacobian(
   }
 
   int jacobian_row_size = 0;
-  jacobian_row_size = 2 * valid_cam_state_ids.size();
+  jacobian_row_size = 4 * valid_cam_state_ids.size();
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
@@ -1458,7 +1469,7 @@ void MsckfVio::twodotFeatureJacobian(
 
     MatrixXd H_xl;
     MatrixXd H_yl;
-    Eigen::VectorXd r_l = VectorXd::Zero(2);
+    Eigen::VectorXd r_l = VectorXd::Zero(4);
 
     twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
     auto cam_state_iter = state_server.cam_states.find(cam_id);
@@ -1468,13 +1479,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, 2) = r_l;
+    r_i.segment(stack_cntr, 4) = r_l;
-    stack_cntr += 2;
+    stack_cntr += 4;
   }
 
   // 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();
@@ -1518,10 +1530,10 @@ void MsckfVio::twodotFeatureJacobian(
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
-
   return;
 }
 
+
 bool MsckfVio::PhotometricPatchPointResidual(
   const StateIDType& cam_state_id,
   const Feature& feature, 
@@ -1628,8 +1640,8 @@ bool MsckfVio::PhotometricPatchPointJacobian(
 
   // calculate derivation for anchor frame, use position for derivation calculation
   // frame derivative calculated convoluting with kernel [-1, 0, 1]
-  dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x");
+  dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x");
-  dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y");
+  dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y");
   //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0,  frame, "Sobel_x") << endl;
 
   dI_dhj(0, 0) = dx * cam0.intrinsics[0];
@@ -1806,6 +1818,8 @@ bool MsckfVio::PhotometricFeatureJacobian(
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
+
+  return false;
   const auto& feature = map_server[feature_id];
 
   // Check how many camera states in the provided camera
@@ -2164,7 +2178,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
 
   // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
-  cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+  // cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
 
   if(FILTER != 0) return;
 
@@ -2247,8 +2261,12 @@ 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)
+  {
+    cout << "zero" << endl;
     return;
+  }
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -2298,6 +2316,8 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
   // Update the IMU state.
   if (FILTER != 2) return;
 
+  cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+
   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]);
 
@@ -2403,7 +2423,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
   VectorXd delta_x = K * r;
   // Update the IMU state.
-  cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+  // cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
 
 
   if (FILTER != 1) return;
@@ -2484,7 +2504,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-  return true;
+  //return true;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
   MatrixXd P2 = Feature::observation_noise *
@@ -2492,8 +2512,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-    cout << "gate: " << dof << " " << gamma << " " <<
+    //cout << "gate: " << dof << " " << gamma << " " <<
-    chi_squared_test_table[dof] << endl;
+    //chi_squared_test_table[dof] << endl;
 
     if (chi_squared_test_table[dof] == 0)
       return false;
@@ -2552,7 +2572,7 @@ void MsckfVio::removeLostFeatures() {
     }
 
     pjacobian_row_size += N*N*feature.observations.size();
-    twojacobian_row_size += 2*feature.observations.size();
+    twojacobian_row_size += 4*feature.observations.size();
     jacobian_row_size += 4*feature.observations.size() - 3;
 
     processed_feature_ids.push_back(feature.id);
@@ -2604,6 +2624,7 @@ void MsckfVio::removeLostFeatures() {
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
+        //cout << "passed" << endl;
         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();
@@ -2619,6 +2640,7 @@ void MsckfVio::removeLostFeatures() {
       stack_cntr += H_xj.rows();
     }  
     if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
+      cout << "passed" << endl;
       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();
@@ -2749,7 +2771,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();
+    twojacobian_row_size += 4*feature.observations.size();
     
   }
 
@@ -2787,6 +2809,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
+        //cout << "passed" << endl;
         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();
@@ -2804,6 +2827,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       }
     
     if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+      cout << "passed" << endl;
       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();
@@ -2929,7 +2953,7 @@ void MsckfVio::pruneCamStateBuffer() {
       }
     }
     
-    twojacobian_row_size += 2*involved_cam_state_ids.size();
+    twojacobian_row_size += 4*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;
   }
@@ -2969,6 +2993,7 @@ void MsckfVio::pruneCamStateBuffer() {
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     { 
         if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
+        //cout << "passed" << endl;
         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();
@@ -2985,6 +3010,7 @@ void MsckfVio::pruneCamStateBuffer() {
     }
 
     if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+      cout << "passed" << endl;
       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();