Debugging two captures of same sensor in a KF....tracker feature dummy

src/processor_tracker_feature_dummy.cpp

@@ -24,13 +24,13 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
         if (features_lost < 2)
         {
             features_lost++;
-            std::cout << "feature " << feat_in_ptr->getMeasurement() << " lost!" << std::endl;
+            std::cout << "feature " << feat_in_ptr->id() << " lost!" << std::endl;
         }
         else
         {
             _feature_list_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
             _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
-            std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl;
+            std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
         }
     }
 
@@ -56,7 +56,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
         n_feature_++;
         new_features_last_.push_back(
                 std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
-        //std::cout << "feature " << new_features_last_.back()->getMeasurement() << " detected!" << std::endl;
+        std::cout << "feature " << new_features_last_.back()->id() << " detected!" << std::endl;
     }
     std::cout << new_features_last_.size() << " features detected!" << std::endl;
 
@@ -66,8 +66,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
 ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
                                                                  FeatureBasePtr _feature_other_ptr)
 {
-    //    std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
-    //              << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
+    std::cout << "creating constraint: last feature " << _feature_ptr->id()
+                          << " with origin feature " << _feature_other_ptr->id() << std::endl;
     auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
     //    _feature_ptr->addConstraint(ctr);
     //    _feature_other_ptr->addConstrainedBy(ctr);

src/test/gtest_processor_base.cpp

@@ -216,42 +216,43 @@ TEST(ProcessorBase, KeyFrameCallback)
 
     CaptureOdom2DPtr capture_odo = make_shared<CaptureOdom2D>(t, sen_odo, Vector2s(0.5,0));
 
-    for (size_t ii=0; ii<10; ii++ )
+    for (size_t ii=0; ii<3; ii++ )
     {
-        WOLF_DEBUG("iter:",ii,"  ts: ", t);
+//        WOLF_DEBUG("iter:",ii,"  ts: ", t);
 
         // Move
         t = t+0.01;
-        capture_odo->setTimeStamp(t);
-        sen_odo->process(capture_odo);
+//        capture_odo->setTimeStamp(t);
+//        sen_odo->process(capture_odo);
 
-        WOLF_DEBUG("iter:",ii,"  ts: ", t);
+//        WOLF_DEBUG("iter:",ii,"  ts: ", t);
 
-        t = t+0.01;
-        capture_odo->setTimeStamp(t);
-        sen_odo->process(capture_odo);
+//        t = t+0.01;
+//        capture_odo->setTimeStamp(t);
+//        sen_odo->process(capture_odo);
 
-        WOLF_DEBUG("iter:",ii,"  ts: ", t);
+//        WOLF_DEBUG("iter:",ii,"  ts: ", t);
 
-        t = t+0.01;
-        capture_odo->setTimeStamp(t);
-        sen_odo->process(capture_odo);
+//        t = t+0.01;
+//        capture_odo->setTimeStamp(t);
+//        sen_odo->process(capture_odo);
 
-        WOLF_DEBUG("iter:",ii,"  ts: ", t);
+//        WOLF_DEBUG("iter:",ii,"  ts: ", t);
 
         // Track
-        proc_tracker->process(make_shared<CaptureVoid>(t, sen_tracker));
+        CaptureVoidPtr cap(make_shared<CaptureVoid>(t, sen_tracker));
+        proc_tracker->process(cap);
 
         problem->print(4,0,0,0);
 
         // Only odom creating KFs
         ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 2D")==0 );
 
-        WOLF_DEBUG("iter:",ii,"  ts: ", t);
+//        WOLF_DEBUG("iter:",ii,"  ts: ", t);
     }
 
     // Print WOLF info
-    problem->print(2);
+//    problem->print(2);
 }
 
 int main(int argc, char **argv)