diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 46efd0cfcc509ad03ee23a747d47cd92b331cd7f..e954ec6807301a2776b179b9b5c1f712ea8d57d9 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -97,20 +97,49 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) override;
-
+ /** \brief First method for matching lmk and feat
+ * \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
+ * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+
+ *
+ * Match the first lmk of the list that respect the given threshold
+ *
+ * \return the number of landmarks found
+ */
unsigned int firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
+ /** \brief Second method for matching lmk and feat
+ * \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
+ * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+
+ *
+ * Match the best(smallest error) lmk of the list that respect the given threshold
+ *
+ * \return the number of landmarks found
+ */
unsigned int bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
-
+ /** \brief Compute errors
+ * \param _w_M_f an isometry world to feature
+ * \param _lmk_obj a pointer of lmk
+ *
+ * A function which compute and return the position and rotation error between lmk and feature.
+ *
+ * \return A pair of errors (position and orientation)
+ */
std::pair<double,double> errorsComputations(Eigen::Isometry3d _w_M_f,
std::shared_ptr<wolf::LandmarkObject> _lmk_obj);
+
unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index ba8d3db2adfcf5cfd4935a9249c7953f4042eecd..790d230a1ba43f20a4a4daef59dcbb08940b5338 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -97,17 +97,14 @@ FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feat
LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
// 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;
+ Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
// 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;
+ Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
// camera to lmk
- pos = _feature_ptr->getMeasurement().head(3);
+ 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;
@@ -225,7 +222,6 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
}
// world to rob
-
Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
// rob to sensor
@@ -246,13 +242,16 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+ //Try matching only if lmk and feat have the same type
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;
-
+
+ //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
if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
_features_out.push_back(feat);
double score(1.0); // not used
@@ -269,6 +268,7 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
std::pair<double,double> ProcessorTrackerLandmarkObject::errorsComputations(Eigen::Isometry3d _w_M_f, std::shared_ptr<wolf::LandmarkObject> _lmk_obj)
{
+
Quaterniond quat_feat;
Eigen::Matrix3d wRf = _w_M_f.linear();
quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
@@ -300,6 +300,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
// rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
+ //If there is not lmk no need to try matching
if (_landmarks_in.size() == 0)
return _features_out.size();
@@ -313,11 +314,17 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
break;
}
+ //A counter for _landmarl_in
int i = 0;
+ //A variable to store the best lmk match
int index_min = -1;
+ //A variable to store store if there was a match or not
bool match = false;
+
+ //Variables to compare errors of lmks
double e_pos_min = 100;
double e_rot_min = 100;
+
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
@@ -326,10 +333,13 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
// 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
if (error.first < e_pos_thr_ && error.second < e_rot_thr_)
- {
+ {
+ //Test if errors are below the previous min errors
if (error.first < e_pos_min && error.second < e_rot_min)
{
@@ -345,18 +355,20 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
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;
- }
+ 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();
@@ -370,6 +382,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
LandmarkMatchMap& _feature_landmark_correspondences)
{
int ftr_size;
+
if (first_lmk_matching_)
{
ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
@@ -394,18 +407,15 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
}
// world to rob
- Vector3d pos_rob_last = getLast()->getFrame()->getP()->getState();
- Quaterniond quat_rob_last (getLast()->getFrame()->getO()->getState().data());
- Eigen::Isometry3d w_M_r_last = Eigen::Translation<double,3>(pos_rob_last.head(3)) * quat_rob_last;
+ Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
- Vector3d pos_rob_incoming = getLast()->getFrame()->getP()->getState();
- Quaterniond quat_rob_incoming (getLast()->getFrame()->getO()->getState().data());
+ 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;
// 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 = posevec_to_isometry(getSensor()->getStateVector("PO"));
+
std::vector<std::pair<std::string, Eigen::Isometry3d> > feats_incoming;
@@ -413,9 +423,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
{
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
- Vector3d pos_obj_incoming = feat_incoming->getMeasurement().head(3);
- Quaterniond quat_obj_incoming (feat_incoming->getMeasurement().tail(4).data());
- Eigen::Isometry3d c_M_f_incoming = Eigen::Translation<double,3>(pos_obj_incoming) * quat_obj_incoming;
+ Eigen::Isometry3d c_M_f_incoming = posevec_to_isometry(feat_incoming->getMeasurement());
Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * c_M_f_incoming;