diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h index e954ec6807301a2776b179b9b5c1f712ea8d57d9..aee02d389e14e1fa3c7b7fae18ba54f2e6a87db0 100644 --- a/include/objectslam/processor/processor_tracker_landmark_object.h +++ b/include/objectslam/processor/processor_tracker_landmark_object.h @@ -85,7 +85,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark void preProcess() override; void postProcess() override; - /** \brief Find provided landmarks as features in the provided capture + /** \brief Select which method for matching(first or best) folowing user parameter * \param _landmarks_in input list of landmarks to be found * \param _capture the capture in which the _landmarks_in should be searched * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp index 790d230a1ba43f20a4a4daef59dcbb08940b5338..0b99ceef5af59889331f75b5d70dfa0cbdfdd408 100644 --- a/src/processor/processor_tracker_landmark_object.cpp +++ b/src/processor/processor_tracker_landmark_object.cpp @@ -61,6 +61,7 @@ void ProcessorTrackerLandmarkObject::postProcess() if (keyframe_voted_){ // Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor for (auto lmk: getProblem()->getMap()->getLandmarkList()){ + auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get(); int number_of_factors = lmk_obj->getConstrainedByList().size(); @@ -103,10 +104,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _ Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO")); // camera to lmk - Vector3d pos = _feature_ptr->getMeasurement().head(3); - Quaterniond quat; - quat.coeffs() = _feature_ptr->getMeasurement().tail(4); - Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat; + Eigen::Isometry3d c_M_t = posevec_to_isometry(_feature_ptr->getMeasurement()); // world to lmk Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t; @@ -245,13 +243,13 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase //Try matching only if lmk and feat have the same type if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) { - // world to feat + //world to feat Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f; //Create a pair that stock both position and rotation error auto error = errorsComputations(w_M_f, lmk_obj); - //Test if the error is below the thresholds + //Test errors are below thresholds if (error.first < e_pos_thr_ && error.second < e_rot_thr_){ _features_out.push_back(feat); double score(1.0); // not used @@ -383,11 +381,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr { int ftr_size; + //First lmk in list (lower than threshold) is matched if (first_lmk_matching_) { ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); } - + + //Best lmk in list (lower than threshold) is matched else { ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); @@ -406,14 +406,12 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture return 0; } - // world to rob + //world to rob Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO")); - Vector3d pos_rob_incoming = getIncoming()->getFrame()->getP()->getState(); - Quaterniond quat_rob_incoming (getIncoming()->getFrame()->getO()->getState().data()); - Eigen::Isometry3d w_M_r_incoming = Eigen::Translation<double,3>(pos_rob_incoming.head(3)) * quat_rob_incoming; + Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO")); - // rob to sensor + //rob to sensor Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO")); @@ -435,7 +433,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture { std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType(); - // camera to feat + //camera to feat Vector3d pos_obj_last = feat_last->getMeasurement().head(3); Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data()); Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;