From 3ae7bdb13af5a1adf283105bf8b1804ed467ffe3 Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Fri, 28 Jun 2019 10:14:32 +0200
Subject: [PATCH] upated two calculation, still not working

---
 launch/msckf_vio_tum.launch | 4 ++--
 src/msckf_vio.cpp           | 5 ++++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 0240de3..b3a66e5 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -17,8 +17,8 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-      <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="false"/>
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="2"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 30ee2e2..d70591a 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -405,7 +405,7 @@ void MsckfVio::imageCallback(
     const sensor_msgs::ImageConstPtr& cam1_img,
     const CameraMeasurementConstPtr& feature_msg)
 {
-   // 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.
@@ -1454,6 +1454,7 @@ void MsckfVio::twodotFeatureJacobian(
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
+
   return;
 }
 
@@ -2341,7 +2342,9 @@ void MsckfVio::removeLostFeatures() {
     cout << "measuring" << endl;
 
     PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
+    cout << "norm" << endl;
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
+    cout << "two" << endl;
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
     cout << "gating" << endl;