Skip to content
Snippets Groups Projects
Commit 56a214bf authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/objectslam into devel
parents 9f238701 476362cd
No related branches found
No related tags found
No related merge requests found
...@@ -144,9 +144,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -144,9 +144,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last, FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming); FeatureBasePtrList& _features_out_incoming);
std::vector<int> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming); static std::pair<std::vector<int>, Eigen::Isometry3d> 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); static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
/** \brief Vote for KeyFrame generation /** \brief Vote for KeyFrame generation
...@@ -220,4 +220,5 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -220,4 +220,5 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
} // namespace wolf } // namespace wolf
#endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */ #endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */
#include <iostream>
#include <ostream>
#include "core/math/rotations.h" #include "core/math/rotations.h"
#include <core/factor/factor_distance_3d.h> #include <core/factor/factor_distance_3d.h>
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h" #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
...@@ -439,9 +442,10 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture ...@@ -439,9 +442,10 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
int index_min_incoming = -1; int index_min_incoming = -1;
double min_e_pos = 100; double min_e_pos = 100;
int index_incoming = 0;
for (auto feat_incoming : detections_incoming_) for (auto feat_incoming : detections_incoming_)
{ {
int index_incoming = 0;
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType(); std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
...@@ -467,20 +471,23 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture ...@@ -467,20 +471,23 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
last_incoming.push_back(tuple_last_incom); last_incoming.push_back(tuple_last_incom);
} }
index_incoming++;
} }
std::cout << index_min_incoming; std::cout << index_min_incoming;
index_last++;
} }
return 1; return 1;
} }
std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming) std::pair<std::vector<int>, Eigen::Isometry3d> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
{ {
// Vector that will contains index of outliers for each iteration // Vector that will contains index of outliers for each iteration
std::vector<std::vector<int> > index_outliers; std::vector<std::vector<int> > index_outliers;
int i = 0; int i = 0;
int best_score = 0; int best_score = 0;
Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
int index_best_score = -1; int index_best_score = -1;
...@@ -506,13 +513,12 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std: ...@@ -506,13 +513,12 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
int index_feat_last = std::get<0>(last_incoming_tuple); int index_feat_last = std::get<0>(last_incoming_tuple);
int index_feat_last_other = std::get<0>(last_incoming_tuple_other); 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) 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 cl_M_ol = std::get<2>(last_incoming_tuple_other);
Eigen::Isometry3d ci_M_oi = std::get<3>(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)) if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{ {
score++; score++;
} }
...@@ -527,6 +533,7 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std: ...@@ -527,6 +533,7 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
{ {
best_score = score; best_score = score;
index_best_score = i; index_best_score = i;
best_cl_M_ci = cl_M_ci;
} }
} }
...@@ -537,16 +544,33 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std: ...@@ -537,16 +544,33 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
} }
if (index_best_score != -1) if (index_best_score != -1)
return index_outliers[index_best_score]; {
std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci};
return p;
}
return index_outliers[0];
std::vector<int> v;
std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
return p;
} }
bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci) std::ostream &operator<<(std::ostream &flux, Quaterniond quat)
{ {
double e_pos_th = 0.5; flux << quat.x() << " ";
double e_rot_th = 0.5; flux << quat.y() << " ";
flux << quat.z() << " ";
flux << quat.w() << " ";
return flux;
}
bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
{
//Thresholds
double e_pos_th = 1;
double e_rot_th = 1;
Eigen::Isometry3d ol_M_cl = cl_M_ol.inverse(); Eigen::Isometry3d ol_M_cl = cl_M_ol.inverse();
Eigen::Isometry3d ol_M_oi = ol_M_cl * cl_M_ci * ci_M_oi; Eigen::Isometry3d ol_M_oi = ol_M_cl * cl_M_ci * ci_M_oi;
...@@ -562,8 +586,10 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen: ...@@ -562,8 +586,10 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose(); quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
Vector3d pos_feat_identity = identity.translation(); Vector3d pos_feat_identity = identity.translation();
std::cout << "\n\n\n\n\n" << quat_feat << " " << quat_feat_identity << std::endl;
std::cout << pos_feat << "\n" << pos_feat_identity << "\n\n\n\n\n" << std::endl;
// Error computation // Error between identity and ol_M_oi
double e_pos = (pos_feat_identity - pos_feat).norm(); double e_pos = (pos_feat_identity - pos_feat).norm();
double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm(); double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
...@@ -595,6 +621,17 @@ void ProcessorTrackerLandmarkObject::resetDerived() ...@@ -595,6 +621,17 @@ void ProcessorTrackerLandmarkObject::resetDerived()
detections_last_ = std::move(detections_incoming_); detections_last_ = std::move(detections_incoming_);
} }
// ostream& operator<<(ostream &flux, Quaterniond quat)
// {
// for (int i = 0; i < quat.size(); i++)
// {
// flux << quat[i];
// flux << " ";
// }
// return flux;
// }
} // namespace wolf } // namespace wolf
......
...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
#include "objectslam/feature/feature_object.h" #include "objectslam/feature/feature_object.h"
#include "objectslam/landmark/landmark_object.h" #include "objectslam/landmark/landmark_object.h"
#include "objectslam/internal/config.h" #include "objectslam/internal/config.h"
#include "objectslam/capture/capture_object.h"
using namespace Eigen; using namespace Eigen;
using namespace wolf; using namespace wolf;
...@@ -309,6 +310,141 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor) ...@@ -309,6 +310,141 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics"); ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
} }
TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
{
std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
Vector7d pose_object_1;
Vector7d pose_object_2;
Vector7d pose_object_3;
Vector7d pose_object_4;
Vector7d pose_object_5;
pose_cam_last << 4.1,3.5,0, 0.8,2.7,1.3,1;
pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
pose_object_2 << 2.2,1.1,3.3,4.5, 2.3,1.4,0.8;
pose_object_3 << 1.2,0.1,3.7,4.5, 2.4,1.4,0.5;
pose_object_4 << 1.8,1.5,1.3,3.5, 3.2,1.0,0.4;
pose_object_5 << 0.8,2.5,2.3,2.5, 1.2,2.0,1.1;
pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
pose_object_3.tail<4>() = pose_object_2.tail<4>().normalized();
pose_object_4.tail<4>() = pose_object_4.tail<4>().normalized();
pose_object_5.tail<4>() = pose_object_5.tail<4>().normalized();
Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
Eigen::Isometry3d w_M_o3 = posevec_to_isometry(pose_object_3);
Eigen::Isometry3d w_M_o4 = posevec_to_isometry(pose_object_4);
Eigen::Isometry3d w_M_o5 = posevec_to_isometry(pose_object_5);
Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
Eigen::Isometry3d cl_M_o3 = w_M_o3;
Eigen::Isometry3d cl_M_o4 = w_M_cl.inverse() * w_M_o4;
Eigen::Isometry3d cl_M_o5 = w_M_o5;
Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
Eigen::Isometry3d ci_M_o3 = w_M_o3;
Eigen::Isometry3d ci_M_o4 = w_M_ci.inverse() * w_M_o4;
Eigen::Isometry3d ci_M_o5 = w_M_o5;
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
auto tuple_o1 = std::make_tuple(0, 0, cl_M_o1, ci_M_o1);
auto tuple_o2 = std::make_tuple(1, 1, cl_M_o2, ci_M_o2);
auto tuple_o3 = std::make_tuple(2, 2, cl_M_o3, ci_M_o3);
auto tuple_o4 = std::make_tuple(3, 3, cl_M_o4, ci_M_o4);
auto tuple_o5 = std::make_tuple(4, 4, cl_M_o5, ci_M_o5);
last_incoming.push_back(tuple_o1);
last_incoming.push_back(tuple_o2);
last_incoming.push_back(tuple_o3);
last_incoming.push_back(tuple_o4);
last_incoming.push_back(tuple_o5);
auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
Quaterniond quat_cam;
Eigen::Matrix3d wRf = cl_M_ci.linear();
quat_cam.coeffs() = R2q(wRf).coeffs().transpose();
Vector3d pos_cam = cl_M_ci.translation();
Quaterniond quat_cam_RANSAC;
Eigen::Matrix3d wRf_R = outliers.second.linear();
quat_cam_RANSAC.coeffs() = R2q(wRf_R).coeffs().transpose();
Vector3d pos_cam_RANSAC = outliers.second.translation();
for (int i = 0; i < pos_cam.size(); i++)
{
ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.0001);
}
ASSERT_TRUE(abs(quat_cam.x() - quat_cam_RANSAC.x()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.y() - quat_cam_RANSAC.y()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.z() - quat_cam_RANSAC.z()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.w() - quat_cam_RANSAC.w()) < 0.0001);
ASSERT_TRUE(outliers.first.size() == 2);
ASSERT_TRUE(outliers.first[0] == 2);
ASSERT_TRUE(outliers.first[1] == 4);
}
TEST(ProcessorTrackerLandmarkObject, isInliers)
{
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
Vector7d pose_object_1;
Vector7d pose_object_2;
pose_cam_last << 4.1,3.5,0, 0.8,2.7,1.3,1;
pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
pose_object_2 << 2.2,1.1,3.3,4.5, 2.3,1.4,0.8;
pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
Eigen::Isometry3d w_M_o3 = Eigen::Isometry3d::Identity();
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
Eigen::Isometry3d cl_M_o3 = w_M_cl.inverse() * w_M_o3;
Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
Eigen::Isometry3d ci_M_o3 = w_M_ci.inverse() * w_M_o3;
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o1, cl_M_ci));
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o2, cl_M_ci));
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o3, ci_M_o3, cl_M_ci));
ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci)));
ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci)));
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
......
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