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