diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h index 4e3103540572024874edde73b63ee42582f129df..827226fba38e862722445282e091681e922c0f8e 100644 --- a/include/objectslam/processor/processor_tracker_landmark_object.h +++ b/include/objectslam/processor/processor_tracker_landmark_object.h @@ -23,7 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm double e_rot_thr_; int fps_; Matrix3d intrinsic_; - bool first_lmk_method_; + bool first_lmk_matching_; ParamsProcessorTrackerLandmarkObject() = default; ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server): @@ -31,7 +31,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm { min_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/min_time_vote"); max_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_vote"); - nb_vote_for_every_first_ = _server.getParam<int>(prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first"); + nb_vote_for_every_first_ = _server.getParam<int> (prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first"); lmk_score_thr_ = _server.getParam<double>(prefix + _unique_name + "/lmk_score_thr"); e_pos_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_pos_thr"); e_rot_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_rot_thr"); @@ -40,7 +40,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm double fy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fy"); double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx"); double cy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cy"); - first_lmk_method_ = _server.getParam<bool>(prefix + _unique_name + "/first_lmk_method"); + first_lmk_matching_ = _server.getParam<bool> (prefix + _unique_name + "/first_lmk_matching"); intrinsic_ << fx, 0, cx, 0, fy, cy, 0, 0, 1; @@ -96,7 +96,12 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark FeatureBasePtrList& _features_out, LandmarkMatchMap& _feature_landmark_correspondences) override; - unsigned int findLandmarksBis(const LandmarkBasePtrList& _landmarks_in, + unsigned int firstLmkMatching(const LandmarkBasePtrList& _landmarks_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + LandmarkMatchMap& _feature_landmark_correspondences); + + unsigned int bestLmkMatching(const LandmarkBasePtrList& _landmarks_in, const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out, LandmarkMatchMap& _feature_landmark_correspondences); @@ -168,7 +173,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark int nb_vote_; int fps_; Matrix3d intrinsic_; - bool first_lmk_method_; + bool first_lmk_matching_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp index 04845b2bc2e899ccd9725ed744cd34f9a9cd2832..f65503802d6e53328cc542bfa1d0a4af6b86f37a 100644 --- a/src/processor/processor_tracker_landmark_object.cpp +++ b/src/processor/processor_tracker_landmark_object.cpp @@ -23,7 +23,7 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT nb_vote_(0), fps_(_params_tracker_landmark_object->fps_), intrinsic_(_params_tracker_landmark_object->intrinsic_), - first_lmk_method_(_params_tracker_landmark_object->first_lmk_method_) + first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_) { std::cout << _params_tracker_landmark_object->print() << std::endl; @@ -214,180 +214,82 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const return false; } -unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in, - const CaptureBasePtr& _capture, - FeatureBasePtrList& _features_out, - LandmarkMatchMap& _feature_landmark_correspondences) +unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBasePtrList& _landmarks_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + LandmarkMatchMap& _feature_landmark_correspondences) { - if (first_lmk_method_) - { - for (auto feat : detections_incoming_) - { - std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); - - if (feat->getCapture() != nullptr){ - break; - } - - for (auto lmk : _landmarks_in) - { - auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); - - if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) - { - - // 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_rot < e_rot_thr_ && e_pos < e_pos_thr_){ - _features_out.push_back(feat); - double score(1.0); - LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score); - _feature_landmark_correspondences.emplace (feat, matched_landmark); - feat->link(_capture); // since all features are created in preProcess() are unlinked - break; - } - } - } - } - - } - else - { - if (!last_ptr_){ + // This is the right thing to test but SEGFAULTS! + if (!last_ptr_){ return 0; - } - - // world to rob - Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); - Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data()); - Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob; - - // rob to sensor - Vector3d pos_sen = getSensor()->getP()->getState(); - Quaterniond quat_sen (getSensor()->getO()->getState().data()); - Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen; - - if (_landmarks_in.size() == 0) - return _features_out.size(); - - for (auto feat : detections_incoming_) - { - std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); - - // camera to feat - Vector3d pos_obj = feat->getMeasurement().head(3); - Quaterniond quat_obj (feat->getMeasurement().tail(4).data()); - Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj; + } - if (feat->getCapture() != nullptr){ - break; - } + // world to rob + Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); + Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data()); + Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob; - std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks; - int i = 0; - int index_lmks = 0; - int index_min = -1; - bool match = false; + // rob to sensor + Vector3d pos_sen = getSensor()->getP()->getState(); + Quaterniond quat_sen (getSensor()->getO()->getState().data()); + Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen; - double e_pos_min = 100; - double e_rot_min = 100; + for (auto feat : detections_incoming_) + { + std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); - for (auto lmk : _landmarks_in) - { - auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); + // camera to feat + Vector3d pos_obj = feat->getMeasurement().head(3); + Quaterniond quat_obj (feat->getMeasurement().tail(4).data()); + Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj; - if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) - { - // 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(); + if (feat->getCapture() != nullptr){ + break; + } - // world to lmk - Vector3d pos_lmk = lmk_obj->getP()->getState(); - Quaterniond quat_lmk(lmk_obj->getO()->getState().data()); + for (auto lmk : _landmarks_in) + { + auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); - auto t = std::make_tuple(i, pos_lmk, quat_lmk); - lmks.push_back(t); + if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) + { + // 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(); - // Error computation - double e_pos = (pos_lmk - pos_feat).norm(); - double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm(); + // world to lmk + Vector3d pos_lmk = lmk_obj->getP()->getState(); + Quaterniond quat_lmk(lmk_obj->getO()->getState().data()); - if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_) - { + // 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_min && e_rot < e_rot_min) - { - e_pos_min = e_pos; - e_rot_min = e_rot; - index_min = std::get<0>(lmks[index_lmks]); - match = true; - } - } - index_lmks++; + if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){ + _features_out.push_back(feat); + double score(1.0); // not used + LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score); + _feature_landmark_correspondences.emplace (feat, matched_landmark); + feat->link(_capture); // since all features are created in preProcess() are unlinked + break; } - i++; - } - - if (match) - { - auto itr_lmk = _landmarks_in.begin(); - std::advance(itr_lmk, index_min); - - _features_out.push_back(feat); - double score(1.0); // not used - LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score); - _feature_landmark_correspondences.emplace (feat, matched_landmark); - feat->link(_capture); // since all features are created in preProcess() are unlinked } } - } - std::cout << "Features Matched :" <<_features_out.size() <<std::endl; return _features_out.size(); } -unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBasePtrList& _landmarks_in, - const CaptureBasePtr& _capture, - FeatureBasePtrList& _features_out, - LandmarkMatchMap& _feature_landmark_correspondences) +unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBasePtrList& _landmarks_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + LandmarkMatchMap& _feature_landmark_correspondences) { - // This is the right thing to test but SEGFAULTS! - if (!last_ptr_){ + if (!last_ptr_) return 0; - } // world to rob Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); @@ -476,127 +378,31 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBase feat->link(_capture); // since all features are created in preProcess() are unlinked } } - - std::cout << "Features Matched :" <<_features_out.size() <<std::endl; + return _features_out.size(); } + -// unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in, -// const CaptureBasePtr& _capture, -// FeatureBasePtrList& _features_out, -// LandmarkMatchMap& _feature_landmark_correspondences) -// { -// std::cout << std::endl << std::endl << std::endl << std::endl << "test11" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - -// if (!last_ptr_){ -// return 0; -// } - -// // world to rob -// Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); -// Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data()); -// Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob; - -// // rob to sensor -// Vector3d pos_sen = getSensor()->getP()->getState(); -// Quaterniond quat_sen (getSensor()->getO()->getState().data()); -// Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen; -// std::cout << std::endl << std::endl << std::endl << std::endl << "test" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - - -// std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks; - -// std::cout << std::endl << std::endl << std::endl << std::endl << "test2" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - - -// int i = 0; - -// std::cout << std::endl << std::endl << std::endl << std::endl << _landmarks_in.size() << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - - -// for (auto lmk : _landmarks_in) -// { -// std::cout << std::endl << std::endl << std::endl << std::endl << "test3" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - -// auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); - -// std::cout << std::endl << std::endl << std::endl << std::endl << "test4" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; - - -// if(lmk_obj != nullptr) -// { -// Vector3d pos_lmk(lmk_obj->getP()->getState()); -// Quaterniond quat_lmk(lmk_obj->getO()->getState().data()); -// auto t = std::make_tuple(i, pos_lmk, quat_lmk); -// lmks.push_back(t); -// std::cout << std::endl << std::endl << std::endl << std::endl << std::get<0>(lmks[i]) << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; -// } - -// i++; -// } - -// for (auto feat : detections_incoming_) -// { -// std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); - -// // camera to feat -// Vector3d pos_obj = feat->getMeasurement().head(3); -// Quaterniond quat_obj (feat->getMeasurement().tail(4).data()); -// Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj; - -// if (feat->getCapture() != nullptr){ -// break; -// } - -// double e_pos_min = 100; -// double e_rot_min = 100; -// int index_min = -1; - -// for (auto lmk : lmks) -// { -// auto itr_lmk = _landmarks_in.begin(); -// std::advance(itr_lmk, std::get<0>(lmk)); - -// auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*itr_lmk); - -// if(lmk_obj->getObjectType() == object_type) -// { -// // 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(); - -// // Error computation -// double e_pos = (std::get<1>(lmk) - pos_feat).norm(); -// double e_rot = log_q(std::get<2>(lmk).conjugate() * quat_feat).norm(); - -// if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_) -// { -// if (e_pos < e_pos_min && e_rot < e_rot_min) -// { -// e_pos_min = e_pos; -// e_rot_min = e_rot; -// index_min = std::get<0>(lmk); -// } -// } -// } -// } - -// auto itr_lmk = _landmarks_in.begin(); -// std::advance(itr_lmk, index_min); - -// _features_out.push_back(feat); -// double score(1.0); // not used -// LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score); -// _feature_landmark_correspondences.emplace (feat, matched_landmark); -// feat->link(_capture); // since all features are created in preProcess() are unlinked -// } -// std::cout << "Features Matched :" <<_features_out.size() <<std::endl; -// return _features_out.size(); -// } +unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in, + const CaptureBasePtr& _capture, + FeatureBasePtrList& _features_out, + LandmarkMatchMap& _feature_landmark_correspondences) +{ + int ftr_size; + if (first_lmk_matching_) + { + ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); + } + + else + { + ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); + } + + std::cout << "Features Matched :" << ftr_size << std::endl; + return ftr_size; +} unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out_last,