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

Add possibility to chose the method for findLandmark

parent 0b8b5dd0
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
+ 8
− 0
View file @ ea1ca540
......@@ -23,6 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double e_rot_thr_;
int fps_;
Matrix3d intrinsic_;
bool first_lmk_method_;
ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
......@@ -39,6 +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");
intrinsic_ << fx, 0, cx,
0, fy, cy,
0, 0, 1;
......@@ -94,6 +96,11 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) override;
unsigned int findLandmarksBis(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
......@@ -161,6 +168,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
int nb_vote_;
int fps_;
Matrix3d intrinsic_;
bool first_lmk_method_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
......
src/processor/processor_tracker_landmark_object.cpp
+ 170
− 100
View file @ ea1ca540
......@@ -22,7 +22,9 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
nb_vote_for_every_first_(_params_tracker_landmark_object->nb_vote_for_every_first_),
nb_vote_(0),
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_)
{
std::cout << _params_tracker_landmark_object->print() << std::endl;
}
......@@ -212,103 +214,172 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
return false;
}
// unsigned int ProcessorTrackerLandmarkObject::findLandmarks(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_){
// return 0;
// }
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(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();
// // 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;
if (feat->getCapture() != nullptr){
break;
}
// // 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;
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// for (auto feat : detections_incoming_)
// {
// std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
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;
}
// // 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;
// 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;
// if (feat->getCapture() != nullptr){
// break;
// }
// 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 << _landmarks_in.size() << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
if (_landmarks_in.size() == 0)
return _features_out.size();
// std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
// int i = 0;
// int index_min = -1;
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
// std::cout << i << index_min << std::endl;
// 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;
// for (auto lmk : _landmarks_in)
// {
// auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if (feat->getCapture() != nullptr){
break;
}
// 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();
std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
int i = 0;
int index_lmks = 0;
int index_min = -1;
bool match = false;
// // world to lmk
// Vector3d pos_lmk = lmk_obj->getP()->getState();
// Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
double e_pos_min = 100;
double e_rot_min = 100;
// auto t = std::make_tuple(i, pos_lmk, quat_lmk);
// lmks.push_back(t);
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// double e_pos_min = 100;
// // double e_rot_min = 100;
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();
// 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();
auto t = std::make_tuple(i, pos_lmk, quat_lmk);
lmks.push_back(t);
// 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_pos_min = e_pos;
// // e_rot_min = e_rot;
// index_min = std::get<0>(lmks[i]);
// }
// i += 1;
if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_)
{
// auto itr_lmk = _landmarks_in.begin();
// std::advance(itr_lmk, index_min);
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++;
}
i++;
}
// _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
// break;
// }
// }
// }
// }
// std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
// return _features_out.size();
// }
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
}
}
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
}
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)
......@@ -328,6 +399,9 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
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();
......@@ -341,19 +415,17 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
break;
}
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;
std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
int i = 0;
int index_lmks = 0;
int index_min = -1;
bool match = false;
std::cout << std::endl << std::endl << std::endl << std::endl << "test1" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
double e_pos_min = 100;
double e_rot_min = 100;
for (auto lmk : _landmarks_in)
{
std::cout << std::endl << std::endl << std::endl << std::endl << "test2" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
......@@ -372,10 +444,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
auto t = std::make_tuple(i, pos_lmk, quat_lmk);
lmks.push_back(t);
double e_pos_min = 100;
double e_rot_min = 100;
// Error computation
double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
......@@ -387,24 +455,26 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
{
e_pos_min = e_pos;
e_rot_min = e_rot;
index_min = std::get<0>(lmks[i]);
index_min = std::get<0>(lmks[index_lmks]);
match = true;
}
}
i++;
index_lmks++;
}
i++;
}
auto itr_lmk = _landmarks_in.begin();
std::advance(itr_lmk, index_min);
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
_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;
......
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