From 8cfbe06945003119b4a9bdd8fa42cb8d0386a6a5 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Tue, 28 May 2019 14:12:22 +0200
Subject: [PATCH] minor calcualtion changes - works at 7-8 fps bagfiles speed

---
 launch/msckf_vio_tum.launch |  2 +-
 src/msckf_vio.cpp           | 14 +++++---------
 2 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 467f3b4..11f6ecc 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="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
       <param name="PrintImages" value="false"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index d8c93e8..7d51d43 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1321,10 +1321,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   r_i[0] = z[0] - und_p_in_c0.x;
   r_i[1] = z[1] - und_p_in_c0.y;
 
-  //cout << "comp\n" << p_c0(0)/p_c0(2) << ":" << und_p_in_c0.x << endl;
-  //cout << p_c0(1)/p_c0(2) << ":" << und_p_in_c0.y << endl;
-
-
   MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
  
   // set anchor Jakobi
@@ -1448,6 +1444,8 @@ void MsckfVio::PhotometricFeatureJacobian(
   H_x = A_null_space.transpose() * H_xi;
   r = A_null_space.transpose() * r_i;
 
+
+
   if(PRINTIMAGES)
   {
     std::cout << "resume playback" << std::endl;
@@ -1581,16 +1579,14 @@ void MsckfVio::featureJacobian(
   FullPivLU<MatrixXd> lu(H_fj.transpose());
   MatrixXd A = lu.kernel();
 
-   H_x = A.transpose() * H_xj;
-   r = A.transpose() * r_j;
+  H_x = A.transpose() * H_xj;
+  r = A.transpose() * r_j;
 
-  /*
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-  myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
+  myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
   myfile.close();
   cout << "---------- LOGGED -------- " << endl; 
-  */
   nh.setParam("/play_bag", false);
 
   return;