From e4dbe2f06074ce4aeaa226cd0510bc8cd79a99ab Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Tue, 21 May 2019 14:23:49 +0200
Subject: [PATCH] removed error in photom. res. calculation

---
 launch/msckf_vio_tum.launch |  2 +-
 src/msckf_vio.cpp           | 41 ++++++++++++++++++++++---------------
 2 files changed, 25 insertions(+), 18 deletions(-)

diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 11f6ecc..467f3b4 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -18,7 +18,7 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="false"/>
+      <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
       <param name="PrintImages" value="false"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 03b1d22..a76c802 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1253,7 +1253,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
 
   // one line of the NxN Jacobians
-  Eigen::Matrix<double, 2, 1> H_rhoj;
+  Eigen::Matrix<double, 2, 3> H_f;
   Eigen::Matrix<double, 2, 6> H_plj;
   Eigen::Matrix<double, 2, 6> H_pAj;
 
@@ -1288,9 +1288,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //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), 
@@ -1298,7 +1298,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
   // Intermediate Jakobians
-  H_rhoj = dh_dGpij * dGpj_drhoj; // 1 x 1
+  H_f   = dh_dCpij * dCpc0_dpg; // 1 x 1
   H_plj = dh_dXplj; // 1 x 6
   H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6
 
@@ -1311,8 +1311,18 @@ void MsckfVio::PhotometricMeasurementJacobian(
   VectorXd r_i = VectorXd::Zero(2);
   
   //calculate residual
-  r_i[0] = z[0] - p_in_c0.x;
-  r_i[1] = z[1] - p_in_c0.y;
+
+  cv::Point2f und_p_in_c0;
+  image_handler::undistortPoints(p_in_c0,
+                                cam0.intrinsics,
+                                cam0.distortion_model,
+                                0,
+                                und_p_in_c0);
+
+  r_i[0] = z[0] - und_p_in_c0.x;
+  r_i[1] = z[1] - und_p_in_c0.y;
+
+  cout << "r:\n" << r_i << endl;
 
   MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
  
@@ -1332,15 +1342,15 @@ void MsckfVio::PhotometricMeasurementJacobian(
   H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj;
 
   H_x = H_xl;
-  H_y = H_rhoj;
+  H_y = H_f;
   r = r_i;
    cout << "h for patch done" << endl;
 
   //TODO make this more fluent as well
-  std::stringstream ss;
-  ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
   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);
   }
@@ -1396,7 +1406,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, 1);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 3);
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1405,20 +1415,17 @@ void MsckfVio::PhotometricFeatureJacobian(
     MatrixXd H_xl;
     MatrixXd H_yl;
     Eigen::VectorXd r_l = VectorXd::Zero(2);
-    cout << "getting jacobi" << endl;
+
     PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
-    cout << "done" << endl;
     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.
-    cout << "stacking" << endl;
     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;
-    cout << "done" << endl;
   }
 
   // Project the residual and Jacobians onto the nullspace
@@ -1597,7 +1604,7 @@ void MsckfVio::measurementUpdate(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();
@@ -1621,10 +1628,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
     //H_thin = Q1.transpose() * H;
     //r_thin = Q1.transpose() * r;
-  } else {
+  } else {*/
     H_thin = H;
     r_thin = r;
-  }
+  //}
 
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;