upated two calculation, still not working

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"/>

src/msckf_vio.cpp

@@ -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;