diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h index 660a5e3238443bb2b7ec343624b0a3826b44a8c1..5df5ab9e7e20816be9445e9c795f7498a4db1862 100644 --- a/include/objectslam/processor/processor_tracker_landmark_object.h +++ b/include/objectslam/processor/processor_tracker_landmark_object.h @@ -110,6 +110,11 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark std::pair<double,double> errorsComputations(Eigen::Isometry3d _w_M_f, std::shared_ptr<wolf::LandmarkObject> _lmk_obj); + + Eigen::Isometry3d getw_M_r(); + Eigen::Isometry3d getr_M_c(); + Eigen::Isometry3d getc_M_f(FeatureBasePtr _feat); + unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture, diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp index 78c6d9fa0ebce4924ba7b23bed691e812f90fece..bd7c9a326f0500dbe1a39312454da2c7cf2e2e8d 100644 --- a/src/processor/processor_tracker_landmark_object.cpp +++ b/src/processor/processor_tracker_landmark_object.cpp @@ -225,23 +225,17 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase } // 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; + Eigen::Isometry3d w_M_r = getw_M_r(); // 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; + Eigen::Isometry3d r_M_c = getr_M_c(); 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; + Eigen::Isometry3d c_M_f = getc_M_f(feat); if (feat->getCapture() != nullptr){ break; @@ -272,6 +266,27 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase return _features_out.size(); } +Eigen::Isometry3d ProcessorTrackerLandmarkObject::getw_M_r() +{ + Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); + Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data()); + return Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob; +} + +Eigen::Isometry3d ProcessorTrackerLandmarkObject::getr_M_c() +{ + Vector3d pos_sen = getSensor()->getP()->getState(); + Quaterniond quat_sen (getSensor()->getO()->getState().data()); + return Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen; +} + +Eigen::Isometry3d ProcessorTrackerLandmarkObject::getc_M_f(FeatureBasePtr _feat) +{ + Vector3d pos_obj = _feat->getMeasurement().head(3); + Quaterniond quat_obj (_feat->getMeasurement().tail(4).data()); + return Eigen::Translation<double,3>(pos_obj) * quat_obj; +} + std::pair<double,double> ProcessorTrackerLandmarkObject::errorsComputations(Eigen::Isometry3d _w_M_f, std::shared_ptr<wolf::LandmarkObject> _lmk_obj) { Quaterniond quat_feat; @@ -296,48 +311,36 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP FeatureBasePtrList& _features_out, LandmarkMatchMap& _feature_landmark_correspondences) { - if (!last_ptr_) + 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; - + Eigen::Isometry3d w_M_r = getw_M_r(); // 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; - + Eigen::Isometry3d r_M_c = getr_M_c(); + 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; - + Eigen::Isometry3d c_M_f = getc_M_f(feat); + if (feat->getCapture() != nullptr){ break; } - - std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks; + int i = 0; - int index_lmks = 0; int index_min = -1; bool match = false; - double e_pos_min = 100; double e_rot_min = 100; - 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 feat @@ -345,38 +348,37 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP auto error = errorsComputations(w_M_f, lmk_obj); - if (error.first < e_pos_thr_ && error.second < e_rot_thr_){ + if (error.first < e_pos_thr_ && error.second < e_rot_thr_) + { + if (error.first < e_pos_min && error.second < e_rot_min) { - if (error.first < e_pos_min && error.second < e_rot_min) - { - e_pos_min = error.first; - e_rot_min = error.second; - index_min = std::get<0>(lmks[index_lmks]); - match = true; - } - } - index_lmks++; + std::cout << std::endl << std::endl << std::endl << std::endl << error.first << " " << error.second << std::endl << std::endl; + + e_pos_min = error.first; + e_rot_min = error.second; + index_min = i; + match = true; + } } - i++; } - + 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 + match = false; } } - - } - + std::cout << "Features Matched :" <<_features_out.size() <<std::endl; return _features_out.size(); }