From cfecefe29f4ef07aab236a831f912f5f43bebf26 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Wed, 17 Apr 2019 17:06:44 +0200
Subject: [PATCH] reinstantiated photometry removed slow-down problem

---
 src/msckf_vio.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 2a1f37a..bc38a4e 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -917,7 +917,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //photometric observation
   std::vector<float> photo_z;
-  //feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
+  feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
 
   // Convert the feature position from the world frame to
   // the cam0 and cam1 frame.
@@ -983,12 +983,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //estimate photometric measurement
   std::vector<float> estimate_photo_z;
-  //feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
+  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
   std::vector<float> photo_r;
   
   //calculate photom. residual
-  //for(int i = 0; i < photo_z.size(); i++)
-  //  photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
+  for(int i = 0; i < photo_z.size(); i++)
+    photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
 
   // printf("-----\n");
   // for(int i = 0; i < photo_z.size(); i++)
@@ -1341,11 +1341,11 @@ void MsckfVio::removeLostFeatures() {
           continue;
         }
       }
-      /*if(!feature.initializeAnchor(cam0_moving_window, cam0))
+      if(!feature.initializeAnchor(cam0_moving_window, cam0))
       {
         invalid_feature_ids.push_back(feature.id);
         continue;
-      }*/
+      }
     }
 
     jacobian_row_size += 4*feature.observations.size() - 3;
@@ -1494,12 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() {
           continue;
         }
       }
-      /*if(!feature.initializeAnchor(cam0_moving_window, cam0))
+      if(!feature.initializeAnchor(cam0_moving_window, cam0))
       {
         for (const auto& cam_id : involved_cam_state_ids)
               feature.observations.erase(cam_id);
         continue;
-      }*/
+      }
     }
 
     jacobian_row_size += 4*involved_cam_state_ids.size() - 3;