some improvements for the demo

src/processor/processor_tracker.cpp

@@ -161,7 +161,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             // eventually add more features
             if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe)
             {
-                WOLF_TRACE("Adding more features...");
+                //WOLF_TRACE("Adding more features...");
                 processNew(params_tracker_->max_new_features);
             }
 
@@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 // We create a KF
 
                 // set KF on last
+                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->setKey();
 
                 // make F; append incoming to new F

src/processor/processor_tracker_landmark_apriltag.cpp

@@ -457,8 +457,11 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const unsigned
                     _new_features_last.erase(it);
                     break; //it should not be possible two detection with the same id before getting there so we can stop here.
                 }
+            // discard features that do not have orientation information
+            if (!std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getUserotation())
+                continue;
 
-            _new_features_last.push_back(feature_in_image); // If the feature is not in the map and not in the list of newly detected features yet then we add it.
+            _new_features_last.push_back(feature_in_image); // If the feature is not in the map & not in the list of newly detected features yet then we add it.
         } //otherwise we check the next feature
     }
 
@@ -759,40 +762,40 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
 
     // std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl;
     // std::cout << "ori_feature_list.size(): " << ori_feature_list.size() << std::endl;
-    if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){
-        return;
-    }
+    
     // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
     Scalar lowest_ration = 1;  // rep_error1/rep_error2 cannot be higher than 1
     FeatureApriltagPtr best_feature;
     bool useable_feature = false;
-    for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
-        FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
-        for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
-            FeatureApriltagPtr ori_feat_ptr =  std::static_pointer_cast<FeatureApriltag>(*it_ori);
-            if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
-                Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); 
-                if (ratio < lowest_ration){
-                // if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
-                    useable_feature = true;
-                    lowest_ration = ratio;
-                    best_feature = last_feat_ptr;
-                    // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
+    if (last_feature_list.size() > 0 && ori_feature_list.size() > 0){
+        for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
+            FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
+            for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
+                FeatureApriltagPtr ori_feat_ptr =  std::static_pointer_cast<FeatureApriltag>(*it_ori);
+                if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
+                    Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); 
+                    //if (ratio < lowest_ration){
+                    if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
+                        useable_feature = true;
+                        lowest_ration = ratio;
+                        best_feature = last_feat_ptr;
+                        // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
+                    }
                 }
             }
         }
     }
     // If there is no common feature between the two images, the continuity is broken and 
     // nothing can be estimated. In the case of aprilslam alone, this result in a broken factor map
-    if (!useable_feature){
+    if (!useable_feature)
         return;
-    }
+    
     // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
     // Retrieve cam to landmark transform
     Eigen::Vector7s cam_pose_lmk = best_feature->getMeasurement();
     Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
     Eigen::Affine3ds cam_M_lmk = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk;
-
+    
     // Get corresponding landmarks in origin/last landmark list
     Eigen::Affine3ds w_M_lmk;
     LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -803,14 +806,13 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
         if (lmk_ptr == nullptr){
             continue;
         }
-
+    
         if (lmk_ptr->getTagId() == best_feature->getTagId()){
             Eigen::Vector3s w_t_lmk = lmk_ptr->getP()->getState();
             Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data());
             w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk;
         }
     }
-
     // Compute last frame estimate
     Eigen::Affine3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
 
@@ -819,7 +821,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
     Eigen::Quaternions quat_last(w_M_last.linear());
     getLast()->getFrame()->getP()->setState(pos_last);
     getLast()->getFrame()->getO()->setState(quat_last.coeffs());
-    
+
     // if (!best_feature->getUserotation()){
     //     return;
     // }

src/solver/solver_manager.cpp

@@ -79,6 +79,8 @@ void SolverManager::update()
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
     for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
     {
+        if (state_blocks_.find(state_ptr)==state_blocks_.end())
+            continue;
         assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
 
         // state update
@@ -106,8 +108,8 @@ void SolverManager::update()
         }
     }
 
-    assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
-    assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
+    //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
+    //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
 }
 
 wolf::ProblemPtr SolverManager::getProblem()

src/trajectory_base.cpp

@@ -82,7 +82,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
 {
     FrameBasePtr closest_kf = nullptr;
     Scalar min_dt = 1e9;
-
+    
     for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
         if ((*frm_rit)->isKey())
         {
@@ -95,6 +95,14 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
             else
                 break;
         }
+    if (closest_kf != nullptr)
+    {
+        WOLF_INFO("TrajectoryBase::closestKeyFrameToTimeStamp: returning KF", closest_kf->id(), " with state ", closest_kf->getState().transpose());
+    }
+    else
+    {
+        WOLF_INFO("TrajectoryBase::closestKeyFrameToTimeStamp: returning nullptr");
+    }
     return closest_kf;
 }