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

Adding comments

parent 7e673b4d
No related branches found
No related tags found
No related merge requests found
...@@ -85,7 +85,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -85,7 +85,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
void preProcess() override; void preProcess() override;
void postProcess() override; void postProcess() override;
/** \brief Find provided landmarks as features in the provided capture /** \brief Select which method for matching(first or best) folowing user parameter
* \param _landmarks_in input list of landmarks to be found * \param _landmarks_in input list of landmarks to be found
* \param _capture the capture in which the _landmarks_in should be searched * \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 _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
......
...@@ -61,6 +61,7 @@ void ProcessorTrackerLandmarkObject::postProcess() ...@@ -61,6 +61,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
if (keyframe_voted_){ if (keyframe_voted_){
// Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor // Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor
for (auto lmk: getProblem()->getMap()->getLandmarkList()){ for (auto lmk: getProblem()->getMap()->getLandmarkList()){
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get(); double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
int number_of_factors = lmk_obj->getConstrainedByList().size(); int number_of_factors = lmk_obj->getConstrainedByList().size();
...@@ -103,10 +104,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _ ...@@ -103,10 +104,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO")); Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
// camera to lmk // camera to lmk
Vector3d pos = _feature_ptr->getMeasurement().head(3); Eigen::Isometry3d c_M_t = posevec_to_isometry(_feature_ptr->getMeasurement());
Quaterniond quat;
quat.coeffs() = _feature_ptr->getMeasurement().tail(4);
Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat;
// world to lmk // world to lmk
Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t; Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
...@@ -245,13 +243,13 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase ...@@ -245,13 +243,13 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
//Try matching only if lmk and feat have the same type //Try matching only if lmk and feat have the same type
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
{ {
// world to feat //world to feat
Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f; Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
//Create a pair that stock both position and rotation error //Create a pair that stock both position and rotation error
auto error = errorsComputations(w_M_f, lmk_obj); auto error = errorsComputations(w_M_f, lmk_obj);
//Test if the error is below the thresholds //Test errors are below thresholds
if (error.first < e_pos_thr_ && error.second < e_rot_thr_){ if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
_features_out.push_back(feat); _features_out.push_back(feat);
double score(1.0); // not used double score(1.0); // not used
...@@ -383,11 +381,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr ...@@ -383,11 +381,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
{ {
int ftr_size; int ftr_size;
//First lmk in list (lower than threshold) is matched
if (first_lmk_matching_) if (first_lmk_matching_)
{ {
ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
} }
//Best lmk in list (lower than threshold) is matched
else else
{ {
ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences); ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
...@@ -406,14 +406,12 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture ...@@ -406,14 +406,12 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 0; return 0;
} }
// world to rob //world to rob
Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO")); Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
Vector3d pos_rob_incoming = getIncoming()->getFrame()->getP()->getState(); Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
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 //rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO")); Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
...@@ -435,7 +433,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture ...@@ -435,7 +433,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
{ {
std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType(); std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
// camera to feat //camera to feat
Vector3d pos_obj_last = feat_last->getMeasurement().head(3); Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data()); Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last; Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_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