From 44fffa19a252244650fbc2ef3075d9b66bb22573 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Tue, 11 Jun 2019 10:59:41 +0200
Subject: [PATCH] minor scale change in distance between pixels

---
 include/msckf_vio/feature.hpp | 10 ++++++++--
 launch/msckf_vio_tum.launch   |  2 +-
 src/msckf_vio.cpp             | 32 ++++----------------------------
 3 files changed, 13 insertions(+), 31 deletions(-)

diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index f5a0d1e..63cfd60 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -874,8 +874,14 @@ cv::Point2f Feature::pixelDistanceAt(
                                 cam.distortion_coeffs, 
                                 pure); 
 
-  // returns the absolute pixel distance at pixels one metres away
-  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
+  // transfrom position to camera frame
+  // to get distance multiplier
+  Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
+  const Eigen::Vector3d& t_c0_w = cam_state.position;
+  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
+
+  // returns the distance between the pixel points in space
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]);
 
   return distance;
 }
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 1d458d2..a28de3f 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 74b689d..a6313b3 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1427,23 +1427,6 @@ void MsckfVio::PhotometricFeatureJacobian(
     nh.setParam("/play_bag", false);
   }
 
-
-  // Errstate
-  calcErrorState();
-  /*
-  std::cout << "--- error state: ---\n " << std::endl;
-  std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl;
-  std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl;
-  
-  int count = 0;
-  for(auto cam_state : err_state_server.cam_states)
-  {
-    std::cout << " - cam " << count++ << " - \n" << std::endl;
-    std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl;
-    std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl;
-  }
-  */
-
   const auto& feature = map_server[feature_id];
   
 
@@ -1493,21 +1476,12 @@ void MsckfVio::PhotometricFeatureJacobian(
   // get Nullspace
   FullPivLU<MatrixXd> lu(H_yi.transpose());
   MatrixXd A_null_space = lu.kernel();
-  /*
-  JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
-  
-  int sv_size = 0;
-  Eigen::VectorXd singularValues = svd_helper.singularValues();
-  for(int i = 0; i < singularValues.size(); i++)
-    if(singularValues[i] > 1e-12)
-      sv_size++;
-
-  MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
-  */
 
   H_x = A_null_space.transpose() * H_xi;
   r = A_null_space.transpose() * r_i;
 
+  cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r)  << endl;
+
   if(PRINTIMAGES)
   {
     ofstream myfile;
@@ -1665,6 +1639,8 @@ void MsckfVio::featureJacobian(
    r = A.transpose() * r_j;
 
 
+  cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
+
       // stop playing bagfile if printing images
   if(PRINTIMAGES)
   {