diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index e954ec6807301a2776b179b9b5c1f712ea8d57d9..fff207e2fe5e2f87ca691968992234a47de51edd 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -85,7 +85,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
void preProcess() 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 _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
@@ -144,6 +144,11 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
+ std::vector<int> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
+
+ bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
+
+
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 790d230a1ba43f20a4a4daef59dcbb08940b5338..93b2411ff3993f5670f957829e4a6f72b19f3f59 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -61,6 +61,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
if (keyframe_voted_){
// Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor
for (auto lmk: getProblem()->getMap()->getLandmarkList()){
+
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
int number_of_factors = lmk_obj->getConstrainedByList().size();
@@ -103,17 +104,15 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
// camera to lmk
- 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;
+ Eigen::Isometry3d c_M_t = posevec_to_isometry(_feature_ptr->getMeasurement());
// world to lmk
Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
// make 7-vector for lmk pose
- pos = w_M_t.translation();
+ auto pos = w_M_t.translation();
Eigen::Matrix3d wRt = w_M_t.linear();
+ Quaterniond quat;
quat.coeffs() = R2q(wRt).coeffs().transpose();
Vector7d w_pose_t;
w_pose_t << pos, quat.coeffs();
@@ -245,13 +244,13 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
//Try matching only if lmk and feat have the same 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;
//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
+ //Test errors are below thresholds
if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
_features_out.push_back(feat);
double score(1.0); // not used
@@ -335,20 +334,23 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
//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)
{
+ // test_counter++;
+
+ // std::cout << "\n\n\n\n\n\n\n\n" << test_counter << std::endl << std::endl;
+
- std::cout << std::endl << std::endl << std::endl << std::endl << error.first << " " << error.second << std::endl << std::endl;
+ // std::cout << "\n\n\n\n\n" << error.first << " " << error.second << std::endl << std::endl;
e_pos_min = error.first;
e_rot_min = error.second;
index_min = i;
- match = true;
+ match = true;
}
}
}
@@ -383,11 +385,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
{
int ftr_size;
+ //First lmk in list (lower than threshold) is matched
if (first_lmk_matching_)
{
ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
}
-
+
+ //Best lmk in list (lower than threshold) is matched
else
{
ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
@@ -406,55 +410,165 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 0;
}
- // world to rob
+ //world to rob last frame
Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
- 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;
+ //world to rob incoming frame
+ Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
- // rob to sensor
+ //rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
-
- std::vector<std::pair<std::string, Eigen::Isometry3d> > feats_incoming;
-
- for (auto feat_incoming : detections_incoming_)
- {
- std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+ int index_last = 0;
- 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;
-
- std::pair<std::string, Eigen::Isometry3d> p(object_type_incoming, w_M_f_incoming);
- feats_incoming.push_back(p);
- }
+ //matches between last and incoming features
+ std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
for (auto feat_last : detections_last_)
{
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);
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 w_M_f_last = w_M_r_last * r_M_c * c_M_f_last;
+ Vector3d pos_feat_last = w_M_f_last.translation();
- std::cout << w_M_f_last(0,0) << std::endl;
+ int index_min_incoming = -1;
+ double min_e_pos = 100;
- for (auto feat_pair_incoming : feats_incoming)
+ for (auto feat_incoming : detections_incoming_)
{
- if (object_type_last == feat_pair_incoming.first)
+ int index_incoming = 0;
+
+ std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+
+ if (object_type_last == object_type_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;
+
+ Vector3d pos_feat_incoming = w_M_f_incoming.translation();
+
+ // Error computation
+ double e_pos = (pos_feat_last - pos_feat_incoming).norm();
+
+ if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
+ {
+ min_e_pos = e_pos;
+ index_min_incoming = index_incoming;
+ }
+
+ auto tuple_last_incom = std::make_tuple(index_last, index_incoming, c_M_f_last, c_M_f_incoming);
+ last_incoming.push_back(tuple_last_incom);
+
+ }
+ }
+
+ std::cout << index_min_incoming;
+
+ }
+ return 1;
+}
+
+std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
+{
+ // Vector that will contains index of outliers for each iteration
+ std::vector<std::vector<int> > index_outliers;
+ int i = 0;
+ int best_score = 0;
+ int index_best_score = -1;
+
+
+ for (auto last_incoming_tuple : last_incoming)
+ {
+
+ std::vector<int> index_outliers_this_iteration;
+
+ int score = 0;
+ int index_feat_incoming = std::get<1>(last_incoming_tuple);
+
+ if (index_feat_incoming != -1)
+ {
+
+ Eigen::Isometry3d c_M_f_last = std::get<2>(last_incoming_tuple);
+ Eigen::Isometry3d f_M_c_incoming = std::get<3>(last_incoming_tuple).inverse();
+
+ Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
+
+ for (auto last_incoming_tuple_other : last_incoming)
{
+
+ int index_feat_last = std::get<0>(last_incoming_tuple);
+ int index_feat_last_other = std::get<0>(last_incoming_tuple_other);
+
+ //Check if there is a match and if
+ if (index_feat_incoming != -1 && index_feat_last != index_feat_last_other)
+ {
+ Eigen::Isometry3d cl_M_ol = std::get<2>(last_incoming_tuple_other);
+ Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other);
+
+ if(isInliers(cl_M_ol, cl_M_ci, ci_M_oi))
+ {
+ score++;
+ }
+ else
+ {
+ index_outliers_this_iteration.push_back(index_feat_last_other);
+ }
+ }
+ }
+ if (score > best_score)
+ {
+ best_score = score;
+ index_best_score = i;
}
+
}
+
+ //Add index of outliers for this iteration of RANSAC
+ index_outliers.push_back(index_outliers_this_iteration);
+ i++;
}
- return 1;
-}
+ if (index_best_score != -1)
+ return index_outliers[index_best_score];
+
+ return index_outliers[0];
+}
+
+bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
+{
+ double e_pos_th = 0.5;
+ double e_rot_th = 0.5;
+
+
+ Eigen::Isometry3d ol_M_cl = cl_M_ol.inverse();
+ Eigen::Isometry3d ol_M_oi = ol_M_cl * cl_M_ci * ci_M_oi;
+
+ Quaterniond quat_feat;
+ Eigen::Matrix3d wRf = ol_M_oi.linear();
+ quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
+ Vector3d pos_feat = ol_M_oi.translation();
+
+ Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
+ Quaterniond quat_feat_identity;
+ Eigen::Matrix3d wRf_i = identity.linear();
+ quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
+ Vector3d pos_feat_identity = identity.translation();
+
+
+ // Error computation
+ double e_pos = (pos_feat_identity - pos_feat).norm();
+ double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
+
+ return (e_pos < e_pos_th && e_rot < e_rot_th);
+}