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
......@@ -23,7 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double e_rot_thr_;
int fps_;
Matrix3d intrinsic_;
bool first_lmk_method_;
bool first_lmk_matching_;
ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
......@@ -31,7 +31,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
{
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");
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");
e_pos_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_pos_thr");
e_rot_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_rot_thr");
......@@ -40,7 +40,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double fy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fy");
double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx");
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,
0, fy, cy,
0, 0, 1;
......@@ -96,7 +96,12 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out,
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,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
......@@ -168,7 +173,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
int nb_vote_;
int fps_;
Matrix3d intrinsic_;
bool first_lmk_method_;
bool first_lmk_matching_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
......
......@@ -23,7 +23,7 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
nb_vote_(0),
fps_(_params_tracker_landmark_object->fps_),
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;
......@@ -214,180 +214,82 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
return false;
}
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
if (first_lmk_method_)
{
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_){
// This is the right thing to test but SEGFAULTS!
if (!last_ptr_){
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){
break;
}
// 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;
std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
int i = 0;
int index_lmks = 0;
int index_min = -1;
bool match = false;
// 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;
double e_pos_min = 100;
double e_rot_min = 100;
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// 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(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;
Quaterniond quat_feat;
Eigen::Matrix3d wRf = w_M_f.linear();
quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
Vector3d pos_feat = w_M_f.translation();
if (feat->getCapture() != nullptr){
break;
}
// world to lmk
Vector3d pos_lmk = lmk_obj->getP()->getState();
Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
auto t = std::make_tuple(i, pos_lmk, quat_lmk);
lmks.push_back(t);
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;
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 = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
// world to lmk
Vector3d pos_lmk = lmk_obj->getP()->getState();
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)
{
e_pos_min = e_pos;
e_rot_min = e_rot;
index_min = std::get<0>(lmks[index_lmks]);
match = true;
}
}
index_lmks++;
if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
_features_out.push_back(feat);
double score(1.0); // not used
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;
}
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();
}
unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
// This is the right thing to test but SEGFAULTS!
if (!last_ptr_){
if (!last_ptr_)
return 0;
}
// world to rob
Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
......@@ -476,127 +378,31 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBase
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();
}
// unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
// const CaptureBasePtr& _capture,
// FeatureBasePtrList& _features_out,
// 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;
// if (!last_ptr_){
// 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;
// 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();
// }
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
int ftr_size;
if (first_lmk_matching_)
{
ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
}
else
{
ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
}
std::cout << "Features Matched :" << ftr_size << std::endl;
return ftr_size;
}
unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment