Skip to content
Snippets Groups Projects
Commit 7e673b4d authored by ydepledt's avatar ydepledt
Browse files

Comment and refactor

parent 87be4b4b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/objectslam/processor/processor_tracker_landmark_object.h
+ 31
− 2
View file @ 7e673b4d
......@@ -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);
......
src/processor/processor_tracker_landmark_object.cpp
+ 40
− 32
View file @ 7e673b4d
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment