Skip to content
Snippets Groups Projects
Commit 443c508e authored by ydepledt's avatar ydepledt
Browse files

Code refactoring

parent ea1ca540
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
+ 10
− 5
View file @ 443c508e
...@@ -23,7 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm ...@@ -23,7 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double e_rot_thr_; double e_rot_thr_;
int fps_; int fps_;
Matrix3d intrinsic_; Matrix3d intrinsic_;
bool first_lmk_method_; bool first_lmk_matching_;
ParamsProcessorTrackerLandmarkObject() = default; ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server): ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
...@@ -31,7 +31,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm ...@@ -31,7 +31,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
{ {
min_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/min_time_vote"); min_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/min_time_vote");
max_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_vote"); max_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_vote");
nb_vote_for_every_first_ = _server.getParam<int>(prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first"); nb_vote_for_every_first_ = _server.getParam<int> (prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first");
lmk_score_thr_ = _server.getParam<double>(prefix + _unique_name + "/lmk_score_thr"); lmk_score_thr_ = _server.getParam<double>(prefix + _unique_name + "/lmk_score_thr");
e_pos_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_pos_thr"); e_pos_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_pos_thr");
e_rot_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_rot_thr"); e_rot_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_rot_thr");
...@@ -40,7 +40,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm ...@@ -40,7 +40,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double fy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fy"); double fy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fy");
double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx"); double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx");
double cy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cy"); double cy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cy");
first_lmk_method_ = _server.getParam<bool>(prefix + _unique_name + "/first_lmk_method"); first_lmk_matching_ = _server.getParam<bool> (prefix + _unique_name + "/first_lmk_matching");
intrinsic_ << fx, 0, cx, intrinsic_ << fx, 0, cx,
0, fy, cy, 0, fy, cy,
0, 0, 1; 0, 0, 1;
...@@ -96,7 +96,12 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -96,7 +96,12 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) override; LandmarkMatchMap& _feature_landmark_correspondences) override;
unsigned int findLandmarksBis(const LandmarkBasePtrList& _landmarks_in, unsigned int firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
unsigned int bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences); LandmarkMatchMap& _feature_landmark_correspondences);
...@@ -168,7 +173,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -168,7 +173,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
int nb_vote_; int nb_vote_;
int fps_; int fps_;
Matrix3d intrinsic_; Matrix3d intrinsic_;
bool first_lmk_method_; bool first_lmk_matching_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
......
src/processor/processor_tracker_landmark_object.cpp
+ 76
− 270
View file @ 443c508e
...@@ -23,7 +23,7 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT ...@@ -23,7 +23,7 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
nb_vote_(0), nb_vote_(0),
fps_(_params_tracker_landmark_object->fps_), fps_(_params_tracker_landmark_object->fps_),
intrinsic_(_params_tracker_landmark_object->intrinsic_), intrinsic_(_params_tracker_landmark_object->intrinsic_),
first_lmk_method_(_params_tracker_landmark_object->first_lmk_method_) first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_)
{ {
std::cout << _params_tracker_landmark_object->print() << std::endl; std::cout << _params_tracker_landmark_object->print() << std::endl;
...@@ -214,180 +214,82 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const ...@@ -214,180 +214,82 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
return false; return false;
} }
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in, unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) LandmarkMatchMap& _feature_landmark_correspondences)
{ {
if (first_lmk_method_) // This is the right thing to test but SEGFAULTS!
{ if (!last_ptr_){
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
if (feat->getCapture() != nullptr){
break;
}
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 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;
// 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;
// camera to feat
pos = feat->getMeasurement().head(3);
quat.coeffs() = feat->getMeasurement().tail(4);
Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
// 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());
// 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_){
_features_out.push_back(feat);
double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
_feature_landmark_correspondences.emplace (feat, matched_landmark);
feat->link(_capture); // since all features are created in preProcess() are unlinked
break;
}
}
}
}
}
else
{
if (!last_ptr_){
return 0; 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;
// 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;
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;
if (feat->getCapture() != nullptr){ // world to rob
break; 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;
std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks; // rob to sensor
int i = 0; Vector3d pos_sen = getSensor()->getP()->getState();
int index_lmks = 0; Quaterniond quat_sen (getSensor()->getO()->getState().data());
int index_min = -1; Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
bool match = false;
double e_pos_min = 100; for (auto feat : detections_incoming_)
double e_rot_min = 100; {
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
for (auto lmk : _landmarks_in) // camera to feat
{ Vector3d pos_obj = feat->getMeasurement().head(3);
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) if (feat->getCapture() != nullptr){
{ break;
// 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 for (auto lmk : _landmarks_in)
Vector3d pos_lmk = lmk_obj->getP()->getState(); {
Quaterniond quat_lmk(lmk_obj->getO()->getState().data()); auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
auto t = std::make_tuple(i, pos_lmk, quat_lmk); if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
lmks.push_back(t); {
// 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();
// Error computation // world to lmk
double e_pos = (pos_lmk - pos_feat).norm(); Vector3d pos_lmk = lmk_obj->getP()->getState();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm(); Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_) // Error computation
{ double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
if (e_pos < e_pos_min && e_rot < e_rot_min) if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
{ _features_out.push_back(feat);
e_pos_min = e_pos; double score(1.0); // not used
e_rot_min = e_rot; LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
index_min = std::get<0>(lmks[index_lmks]); _feature_landmark_correspondences.emplace (feat, matched_landmark);
match = true; feat->link(_capture); // since all features are created in preProcess() are unlinked
} break;
}
index_lmks++;
} }
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
} }
} }
} }
std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
return _features_out.size(); return _features_out.size();
} }
unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBasePtrList& _landmarks_in, unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) LandmarkMatchMap& _feature_landmark_correspondences)
{ {
// This is the right thing to test but SEGFAULTS! if (!last_ptr_)
if (!last_ptr_){
return 0; return 0;
}
// world to rob // world to rob
Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
...@@ -476,127 +378,31 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBase ...@@ -476,127 +378,31 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBase
feat->link(_capture); // since all features are created in preProcess() are unlinked feat->link(_capture); // since all features are created in preProcess() are unlinked
} }
} }
std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
return _features_out.size(); return _features_out.size();
} }
// unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in, unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
// const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
// FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
// LandmarkMatchMap& _feature_landmark_correspondences) LandmarkMatchMap& _feature_landmark_correspondences)
// { {
// std::cout << std::endl << std::endl << std::endl << std::endl << "test11" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; int ftr_size;
if (first_lmk_matching_)
// if (!last_ptr_){ {
// return 0; ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
// } }
// // world to rob else
// Vector3d pos_rob = getLast()->getFrame()->getP()->getState(); {
// Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data()); ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
// Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob; }
// // 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;
// std::cout << std::endl << std::endl << std::endl << std::endl << "test" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
// std::cout << std::endl << std::endl << std::endl << std::endl << "test2" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// int i = 0;
// std::cout << std::endl << std::endl << std::endl << std::endl << _landmarks_in.size() << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// for (auto lmk : _landmarks_in)
// {
// std::cout << std::endl << std::endl << std::endl << std::endl << "test3" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// std::cout << std::endl << std::endl << std::endl << std::endl << "test4" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// if(lmk_obj != nullptr)
// {
// 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);
// std::cout << std::endl << std::endl << std::endl << std::endl << std::get<0>(lmks[i]) << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
// }
// i++;
// }
// 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;
// if (feat->getCapture() != nullptr){
// break;
// }
// double e_pos_min = 100;
// double e_rot_min = 100;
// int index_min = -1;
// for (auto lmk : lmks)
// {
// auto itr_lmk = _landmarks_in.begin();
// std::advance(itr_lmk, std::get<0>(lmk));
// auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*itr_lmk);
// if(lmk_obj->getObjectType() == object_type)
// {
// // 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();
// // Error computation
// double e_pos = (std::get<1>(lmk) - pos_feat).norm();
// double e_rot = log_q(std::get<2>(lmk).conjugate() * quat_feat).norm();
// if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_)
// {
// if (e_pos < e_pos_min && e_rot < e_rot_min)
// {
// e_pos_min = e_pos;
// e_rot_min = e_rot;
// index_min = std::get<0>(lmk);
// }
// }
// }
// }
// 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
// }
// std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
// return _features_out.size();
// }
std::cout << "Features Matched :" << ftr_size << std::endl;
return ftr_size;
}
unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture, unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last, FeatureBasePtrList& _features_out_last,
......
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