From 4e40c1ce3c8fadcd2452963e6688fa003b1e9247 Mon Sep 17 00:00:00 2001
From: Cesar Debeunne <cdebeunne@nilgiri.laas.fr>
Date: Fri, 20 Aug 2021 14:02:35 +0200
Subject: [PATCH] cleaning

---
 demos/cosyslam_imu.cpp                        |  6 --
 demos/yaml/cosyslam_imu.yaml                  |  2 +-
 .../processor_tracker_landmark_object.h       |  1 +
 .../processor_tracker_landmark_object.cpp     | 90 ++++++-------------
 4 files changed, 28 insertions(+), 71 deletions(-)

diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index 5b2137d..ee95329 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -162,7 +162,6 @@ int main()
     foreach(rosbag::MessageInstance const m, view)
     {
         std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
-        std::cout << problem->getLastFrame()->getP()->getState() << std::endl;
 
         wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
         if (cosyArray != nullptr){
@@ -179,11 +178,6 @@ int main()
                 sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
                 cov = sig.array().matrix().asDiagonal();
                 ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name};
-                std::cout << "OBJECT DETECTED" << std::endl;
-                std::cout << object.name << std::endl;
-                std::cout << poseVec << std::endl;
-                std::cout << object.score << std::endl;
-                std::cout << "------------" << std::endl;
                 dets.push_back(det);
             }
             TimeStamp ts(m.getTime().toSec());
diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml
index 063a9c0..aa4da67 100644
--- a/demos/yaml/cosyslam_imu.yaml
+++ b/demos/yaml/cosyslam_imu.yaml
@@ -1,5 +1,5 @@
 # rosbag name
-bag: "/demos/bag/demo_imu_long_wolf_ws.bag"
+bag: "/demos/bag/demo_imu_wolf.bag"
 
 # Camera to IMU transformation
 unfix_extrinsic: false
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index dfb3ab9..df7bae2 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -166,6 +166,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
         unsigned int    min_features_for_keyframe_;
         int             nb_vote_for_every_first_;
         bool            add_3d_cstr_;
+        bool            keyframe_voted_;
         int             nb_vote_;
         int             fps_;
         Matrix3d        intrinsic_;
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 0d0fc9b..c3d6dae 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -40,6 +40,7 @@ void ProcessorTrackerLandmarkObject::preProcess()
 {
     //clear wolf detections so that new ones will be stored inside
     detections_incoming_.clear();
+    keyframe_voted_ = false;
 
     auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_);
     assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject");
@@ -56,31 +57,34 @@ void ProcessorTrackerLandmarkObject::postProcess()
 {
     nb_vote_++;
 
-    // Clear the landmark map 
-    LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
-    for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){
-        auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*iter_map);
-        double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
-        int number_of_factors = lmk_obj->getConstrainedByList().size();
-        double lmk_score = number_of_factors / dt_lmk;
-        std::cout << lmk_obj->getObjectType() << std::endl;
-        std::cout << lmk_score << std::endl;
-
-        if (number_of_factors > 10){
-            continue;
-        }
+    // Clear the landmark map
+    if (keyframe_voted_){ 
+        // Only if a keyframe is voted so that a suppressed landmark is not assigned to
+        // a factor 
+        LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
+        for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){
+            auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*iter_map);
+            double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
+            int number_of_factors = lmk_obj->getConstrainedByList().size();
+            double lmk_score = number_of_factors / dt_lmk;
+
+            if (number_of_factors > 10){
+                lmk_score = 10;
+            }
 
-        if (lmk_score < lmk_score_thr_){
-            iter_map->get()->remove();
+            if (lmk_score < lmk_score_thr_){
+                iter_map->get()->remove();
+            }
         }
-
     }
-    
 }
 
 FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr,
                                                             LandmarkBasePtr _landmark_ptr)
 {
+    // A keyframe is voted 
+    keyframe_voted_ = true;
+
     return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr,
                                                                 getSensor(),
                                                                 getLast()->getFrame(),
@@ -141,57 +145,12 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
         bool feature_already_found = false;
         auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
 
+        // If the feature was linked, there is a capture
         if (feat->getCapture() != nullptr){
             feature_already_found = true;
             break;
         }
 
-        // Loop over the landmark to find if one is associated to feat
-        for(auto lmk: lmk_lst){
-            LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
-
-            // Check if the object ID is the same
-            if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){
-
-                // Then check if the world pose is similar
-
-                // world to rob
-                Vector3d pos = getLast()->getFrame()->getP()->getState();
-                Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
-                Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                // rob to sensor
-                pos = getSensor()->getP()->getState();
-                quat.coeffs() = getSensor()->getO()->getState();
-                Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                // camera to feat
-                pos = feat->getMeasurement().head(3);
-                quat.coeffs() = feat->getMeasurement().tail(4);
-                Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
-
-                // world to feat
-                Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
-                Quaterniond quat_feat;
-                Eigen::Matrix3d wRf = w_M_f.linear();
-                quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
-                Vector3d pos_feat = w_M_f.translation();
-
-                // world to lmk
-                Vector3d pos_lmk = lmk_obj->getP()->getState();
-                Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
-
-                // Error computation
-                double e_pos = (pos_lmk - pos_feat).norm();
-                double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
-
-                if (e_pos < e_pos_thr_ && e_rot < e_rot_thr_){
-                    feature_already_found = true;
-                    break;   
-                }      
-            }
-        }
-
         if (!feature_already_found)
         {
             // If the feature is not in the map & not in the list of newly detected features yet, then we add it.
@@ -225,9 +184,11 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
     if (too_long_since_last_KF){
         return true;
     }
+
     // no detection in incoming capture and a minimum time since last KF has past
-    if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote)
+    if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote){
         return true;
+    }
 
     // Vote for every image processed at the beginning if possible
     if (nb_vote_ < nb_vote_for_every_first_){
@@ -253,6 +214,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
         for (auto lmk : _landmarks_in)
         {   
             auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+
             if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
             {
                 
-- 
GitLab