From 9fba952b51e79c537c63219fd7897930708cc638 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Wed, 15 Jun 2022 18:05:59 +0200
Subject: [PATCH] Add test for matching functions
---
.../processor_tracker_landmark_object.h | 5 +-
.../processor_tracker_landmark_object.cpp | 57 +++++++--
...test_processor_tracker_landmark_object.cpp | 118 ++++++++++++++++++
3 files changed, 168 insertions(+), 12 deletions(-)
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index fff207e..c03add8 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -144,9 +144,9 @@ 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);
+ 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
@@ -220,4 +220,5 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
} // namespace wolf
+
#endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 93b2411..043357c 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -1,3 +1,6 @@
+#include <iostream>
+#include <ostream>
+
#include "core/math/rotations.h"
#include <core/factor/factor_distance_3d.h>
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
@@ -439,9 +442,10 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
int index_min_incoming = -1;
double min_e_pos = 100;
+ int index_incoming = 0;
+
for (auto feat_incoming : detections_incoming_)
{
- int index_incoming = 0;
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
@@ -467,20 +471,23 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
last_incoming.push_back(tuple_last_incom);
}
+ index_incoming++;
}
std::cout << index_min_incoming;
+ index_last++;
}
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
std::vector<std::vector<int> > index_outliers;
int i = 0;
int best_score = 0;
+ Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
int index_best_score = -1;
@@ -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_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))
+ if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{
score++;
}
@@ -527,6 +533,7 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
{
best_score = score;
index_best_score = i;
+ best_cl_M_ci = cl_M_ci;
}
}
@@ -537,16 +544,33 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
}
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;
- double e_rot_th = 0.5;
+ flux << quat.x() << " ";
+ 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_oi = ol_M_cl * cl_M_ci * ci_M_oi;
@@ -562,8 +586,10 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
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_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
@@ -595,6 +621,17 @@ void ProcessorTrackerLandmarkObject::resetDerived()
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
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 80727dc..0da8745 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -10,6 +10,7 @@
#include "objectslam/feature/feature_object.h"
#include "objectslam/landmark/landmark_object.h"
#include "objectslam/internal/config.h"
+#include "objectslam/capture/capture_object.h"
using namespace Eigen;
using namespace wolf;
@@ -311,6 +312,123 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
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;
+
+ 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_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();
+
+
+ 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 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 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 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);
+
+ last_incoming.push_back(tuple_o1);
+ last_incoming.push_back(tuple_o2);
+ last_incoming.push_back(tuple_o3);
+
+ 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();
+
+ std::cout << "\n\n\n\n\n" << outliers.first.size() << "\n\n\n\n";
+
+ std::cout << "\n\n\n\n\n\n\n\n" << pos_cam << "\n" << pos_cam_RANSAC << "\n\n";
+
+ for (int i = 0; i < pos_cam.size(); i++)
+ {
+ ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.001);
+ }
+
+ ASSERT_TRUE(outliers.first.size() == 1);
+ ASSERT_TRUE(outliers.first[0] == 2);
+}
+
+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)
{
--
GitLab