diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 827226fba38e862722445282e091681e922c0f8e..660a5e3238443bb2b7ec343624b0a3826b44a8c1 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -5,6 +5,8 @@
#include <core/common/wolf.h>
#include <core/processor/processor_tracker_landmark.h>
#include <core/factor/factor_distance_3d.h>
+#include "objectslam/landmark/landmark_object.h"
+
namespace wolf
@@ -106,6 +108,10 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
+ 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 f65503802d6e53328cc542bfa1d0a4af6b86f37a..78c6d9fa0ebce4924ba7b23bed691e812f90fece 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -255,20 +255,10 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
{
// 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());
+ auto error = errorsComputations(w_M_f, lmk_obj);
- // 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_){
+ if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
_features_out.push_back(feat);
double score(1.0); // not used
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
@@ -282,6 +272,24 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
return _features_out.size();
}
+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();
+ 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();
+
+ std::pair<double,double> error = std::make_pair(e_pos, e_rot);
+
+ return error;
+}
+
unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
@@ -334,52 +342,43 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
{
// 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());
-
- auto t = std::make_tuple(i, pos_lmk, quat_lmk);
- lmks.push_back(t);
-
- // 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_)
- {
+ auto error = errorsComputations(w_M_f, lmk_obj);
- if (e_pos < e_pos_min && e_rot < e_rot_min)
+ if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
{
- e_pos_min = e_pos;
- e_rot_min = e_rot;
- index_min = std::get<0>(lmks[index_lmks]);
- match = true;
- }
+
+ 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++;
}
- index_lmks++;
+ 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
+ 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
+ }
}
+
+
}
return _features_out.size();
+
}