diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 734fe656a8a60ae094151832cbc6e46a697c5ba4..64ce7788b8a96f17b4b6470debdb5181d6dcc836 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -219,8 +219,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*
*/
static void processFeatures(const std::vector<std::pair<int,int> >& _matches,
- FeatureBasePtrList detections_last,
- FeatureBasePtrList detections_incoming,
+ const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming,
TrackMatrix& _trackMatrix);
/** \brief Filter matches thanks to outliers vector
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 245dc1989203d4e10c370306842776ac19e738c0..cf2ea9ed12e26643792b61c30cdfad668e333d27 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -594,14 +594,14 @@ std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::createAllMatche
void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches,
- FeatureBasePtrList detections_last,
- FeatureBasePtrList detections_incoming,
+ const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming,
TrackMatrix& _trackMatrix)
{
for (auto match : _matches)
{
- auto feat_last_itr = detections_last.begin();
- auto feat_incoming_itr = detections_incoming.begin();
+ auto feat_last_itr = _features_out_last.begin();
+ auto feat_incoming_itr = _features_out_incoming.begin();
std::advance(feat_last_itr, match.first);
std::advance(feat_incoming_itr, match.second);
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 588e6b43917971473f72cddd9701e5b3954760a4..7a0f8c2f23398ff05413ed741d64b07f38a62325 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -35,19 +35,89 @@ Eigen::Isometry3d appendFeatureLast(const std::string& _object_type, Eigen::Vect
return cl_M_f;
}
+FeatureBasePtr createFeatureLast(const std::string& _object_type, Eigen::Vector7d& _pose,
+ Eigen::Isometry3d& _cl_M_f,
+ CaptureBasePtr& _capture,
+ FeatureBasePtrList& _feature_last,
+ TrackMatrix& _trackmatrix, bool _intoTrackMatrix)
+
+{
+ _pose.tail<4>() = _pose.tail<4>().normalized();
+ _cl_M_f = posevec_to_isometry(_pose);
+ FeatureBasePtr f = std::make_shared<FeatureObject>(_pose, Matrix6d::Identity(), _object_type);
+ _feature_last.push_back(f);
+
+ f->link(_capture);
+
+ if (_intoTrackMatrix)
+ {
+ _trackmatrix.newTrack(f);
+ }
+
+ return f;
+}
+
+FeatureBasePtr createFeatureLast(const std::string& _object_type, Eigen::Vector7d& _pose,
+ Eigen::Isometry3d& _cl_M_f,
+ CaptureBasePtr& _capture,
+ TrackMatrix& _trackmatrix, bool _intoTrackMatrix)
+
+{
+ _pose.tail<4>() = _pose.tail<4>().normalized();
+ _cl_M_f = posevec_to_isometry(_pose);
+ FeatureBasePtr f = std::make_shared<FeatureObject>(_pose, Matrix6d::Identity(), _object_type);
+
+ f->link(_capture);
+
+ if (_intoTrackMatrix)
+ {
+ _trackmatrix.newTrack(f);
+ }
+
+ return f;
+}
+
+
+void appendFeatureIncoming(const std::string& _object_type,
+ const Eigen::Isometry3d& _cl_M_f,
+ const Eigen::Isometry3d& _ci_M_cl,
+ FeatureBasePtrList& _features_incoming)
-void appendFeaturencoming(const std::string& _object_type, const Eigen::Isometry3d& _cl_M_f, const Eigen::Isometry3d& _ci_M_cl, FeatureBasePtrList& _features_incoming)
{
Eigen::Isometry3d ci_M_f = _ci_M_cl * _cl_M_f;
Eigen::Quaterniond quat(ci_M_f.linear());
Eigen::Vector3d pos = ci_M_f.translation();
Eigen::Vector7d pose;
pose << pos, quat.coeffs();
+
FeatureBasePtr f = std::make_shared<FeatureObject>(pose, Matrix6d::Identity(), _object_type);
+
_features_incoming.push_back(f);
}
+FeatureBasePtr createFeatureIncoming(const std::string& _object_type,
+ const Eigen::Isometry3d& _cl_M_f,
+ const Eigen::Isometry3d& _ci_M_cl,
+ CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_incoming)
+
+{
+ Eigen::Isometry3d ci_M_f = _ci_M_cl * _cl_M_f;
+ Eigen::Quaterniond quat(ci_M_f.linear());
+ Eigen::Vector3d pos = ci_M_f.translation();
+ Eigen::Vector7d pose;
+ pose << pos, quat.coeffs();
+
+ FeatureBasePtr f = std::make_shared<FeatureObject>(pose, Matrix6d::Identity(), _object_type);
+ f->link(_capture);
+
+ _features_incoming.push_back(f);
+
+ return f;
+
+}
+
class ProcessorTrackerLandmarkObject_Wrapper : public ProcessorTrackerLandmarkObject
{
@@ -110,11 +180,46 @@ public:
VectorComposite s0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
F1 = problem->setPriorFactor(x0, s0, 0.0);
+ F2 = FrameBase::emplace<FrameBase>(problem->getTrajectory(),
+ 1.0,
+ "PO",
+ std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
+
+ F3 = FrameBase::emplace<FrameBase>(problem->getTrajectory(),
+ 2.0,
+ "PO",
+ std::list<VectorXd>{Vector2d(2,0), Vector1d(0)});
+
// minimal config for the processor to be operative
- C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());
+ C1 = CaptureBase::emplace<CapturePose>(F1, 0.0, sen, Vector7d(), Matrix6d());
prc_obj->setOriginPtr(C1);
prc_obj->setLastPtr(C1);
+ C2 = CaptureBase::emplace<CapturePose>(F2, 1.0, sen, Vector7d(), Matrix6d());
+
+ C3 = CaptureBase::emplace<CapturePose>(F3, 2.0, sen, Vector7d(), Matrix6d());
+
+ // C1 = CaptureBase::emplace<CaptureDiffDrive>(F1,
+ // 0.0,
+ // sen,
+ // data1.Zero(),
+ // data_cov,
+ // nullptr);
+
+ // C2 = CaptureBase::emplace<CaptureDiffDrive>(F2,
+ // 1.0,
+ // sen,
+ // data1.Zero(),
+ // data_cov,
+ // nullptr);
+
+ // C3 = CaptureBase::emplace<CaptureDiffDrive>(F3,
+ // 2.0,
+ // sen,
+ // data1.Zero(),
+ // data_cov,
+ // nullptr);
+
// Give random values
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;
@@ -123,7 +228,7 @@ public:
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_object_5_ << 0.8, 2.5, 2.3, 2.5, 1.2, 2.0, 1.1;
// Normalizing quaternions
pose_cam_last_.tail<4>() = pose_cam_last_.tail<4>().normalized();
@@ -143,7 +248,12 @@ public:
SensorBasePtr sen;
ProcessorBasePtr prc;
FrameBasePtr F1;
+ FrameBasePtr F2;
+ FrameBasePtr F3;
CaptureBasePtr C1;
+ CaptureBasePtr C2;
+ CaptureBasePtr C3;
+
// define camera and object poses
Vector7d pose_cam_last_;
@@ -607,25 +717,25 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, allMatchesSameType)
Eigen::Isometry3d ci_M_cl = w_M_ci.inverse() * w_M_cl;
// feature 0
- appendFeaturencoming("type0", cl_M_f1, ci_M_cl, features_incoming); // go with f1
+ appendFeatureIncoming("type0", cl_M_f1, ci_M_cl, features_incoming); // go with f1
// feature 1
- appendFeaturencoming("type0", cl_M_f2, ci_M_cl, features_incoming); // go with f2
+ appendFeatureIncoming("type0", cl_M_f2, ci_M_cl, features_incoming); // go with f2
// feature 2
- appendFeaturencoming("type2", cl_M_f3, ci_M_cl, features_incoming); // go with f3
+ appendFeatureIncoming("type2", cl_M_f3, ci_M_cl, features_incoming); // go with f3
// feature 3
- appendFeaturencoming("type0", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
+ appendFeatureIncoming("type0", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 4
- appendFeaturencoming("type1", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
+ appendFeatureIncoming("type1", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 5
- appendFeaturencoming("type1", cl_M_f5, ci_M_cl, features_incoming); // go with f5
+ appendFeatureIncoming("type1", cl_M_f5, ci_M_cl, features_incoming); // go with f5
// feature 6
- appendFeaturencoming("type1", cl_M_f4, ci_M_cl, features_incoming); // go with f4
+ appendFeatureIncoming("type1", cl_M_f4, ci_M_cl, features_incoming); // go with f4
auto all_matches = ProcessorTrackerLandmarkObject::createAllMatchesSameType(features_last, features_incoming, cl_M_o_vec, ci_M_o_vec);
@@ -662,6 +772,113 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, allMatchesSameType)
ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(2, 2)));
ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(3, 6)));
ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 5)));
+
+}
+
+TEST_F(ProcessorTrackerLandmarkObject_fixture, processFeatures)
+{
+ std::vector<std::pair<int,int> > matches;
+ FeatureBasePtrList features_last;
+ FeatureBasePtrList features_incoming;
+ Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
+ std::vector<Eigen::Isometry3d> cl_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
+ TrackMatrix trackMatrix;
+
+ // feature 1
+ Eigen::Isometry3d cl_M_f1;
+ auto f1 = createFeatureLast("type0", pose_object_1_, cl_M_f1, C1, trackMatrix, true);
+
+ // feature 2
+ Eigen::Isometry3d cl_M_f2;
+ auto f2 = createFeatureLast("type1", pose_object_2_, cl_M_f2, C1, trackMatrix, true);
+
+ // feature 3
+ Eigen::Isometry3d cl_M_f3;
+ auto f3 = createFeatureLast("type0", pose_object_3_, cl_M_f3, C2, features_last, trackMatrix, false);
+ trackMatrix.add(f1, f3);
+
+ // feature 4
+ Eigen::Isometry3d cl_M_f4;
+ auto f4 = createFeatureLast("type1", pose_object_4_, cl_M_f4, C2, features_last, trackMatrix, false);
+ trackMatrix.add(f2, f4);
+
+ // feature 5
+ Eigen::Isometry3d cl_M_f5;
+ auto f5 = createFeatureLast("type2", pose_object_5_, cl_M_f5, C2, features_last, trackMatrix, false);
+
+
+ Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last_);
+ Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming_);
+ Eigen::Isometry3d ci_M_cl = w_M_ci.inverse() * w_M_cl;
+
+ // feature 1
+ auto f1_incoming = createFeatureIncoming("type0", cl_M_f3, ci_M_cl, C3, features_incoming); // go with f3
+
+ // feature 2
+ auto f2_incoming = createFeatureIncoming("type1", cl_M_f4, ci_M_cl, C3, features_incoming); // go with f4
+
+ // feature 3
+ auto f3_incoming = createFeatureIncoming("type2", cl_M_f5, ci_M_cl, C3, features_incoming); // go with f3
+
+ // feature 4
+ auto f4_incoming = createFeatureIncoming("type0", cl_M_f1, Eigen::Isometry3d::Identity(), C3, features_incoming); //outliers
+
+ // feature 5
+ auto f5_incoming = createFeatureIncoming("type1", cl_M_f1, Eigen::Isometry3d::Identity(), C3, features_incoming); //outliers
+
+ // feature 6
+ auto f6_incoming = createFeatureIncoming("type1", cl_M_f1, ci_M_cl, C3, features_incoming); // outliers
+
+
+ auto all_matches = ProcessorTrackerLandmarkObject::createAllMatchesSameType(features_last, features_incoming, cl_M_o_vec, ci_M_o_vec);
+ // 2 (type0) + 3 (type1) + 1 (type2) = 6
+ ASSERT_TRUE(all_matches.size() == 6);
+
+ ProcessorTrackerLandmarkObject::keepInliers(cl_M_o_vec, ci_M_o_vec, all_matches, best_model);
+
+ ProcessorTrackerLandmarkObject::processFeatures(all_matches, features_last, features_incoming, trackMatrix);
+
+ auto trackIdFeat1 = f1->trackId();
+ auto trackIdFeat2 = f2->trackId();
+ auto trackIdFeat3 = f3->trackId();
+ auto trackIdFeat4 = f4->trackId();
+ auto trackIdFeat5 = f5->trackId();
+
+ auto sizeTrackFeat1 = trackMatrix.trackSize(trackIdFeat1);
+ auto sizeTrackFeat2 = trackMatrix.trackSize(trackIdFeat2);
+ auto sizeTrackFeat3 = trackMatrix.trackSize(trackIdFeat3);
+ auto sizeTrackFeat4 = trackMatrix.trackSize(trackIdFeat4);
+ auto sizeTrackFeat5 = trackMatrix.trackSize(trackIdFeat5);
+
+
+ // * State of the trackMatrix
+
+ // * C0 C1 C2
+ // * Tracks for each matched feature:
+ // * f1 --- f3 --- f1_i <-- track 1 of corresponding features in different captures
+ // * | | |
+ // * f2 --- f4 --- f2_i <-- track 2
+ // * | | |
+ // * | f5 --- f3_i <-- track 3
+
+
+ //Test if the trackMatrix is the one above
+
+ ASSERT_TRUE(sizeTrackFeat1 == 3);
+ ASSERT_TRUE(sizeTrackFeat2 == 3);
+ ASSERT_TRUE(sizeTrackFeat3 == 3);
+ ASSERT_TRUE(sizeTrackFeat4 == 3);
+ ASSERT_TRUE(sizeTrackFeat5 == 2);
+
+ ASSERT_TRUE(trackMatrix.firstFeature(trackIdFeat1) == f1);
+ ASSERT_TRUE(trackMatrix.lastFeature(trackIdFeat1) == f1_incoming);
+
+ ASSERT_TRUE(trackMatrix.firstFeature(trackIdFeat2) == f2);
+ ASSERT_TRUE(trackMatrix.lastFeature(trackIdFeat2) == f2_incoming);
+
+ ASSERT_TRUE(trackMatrix.firstFeature(trackIdFeat5) == f5);
+ ASSERT_TRUE(trackMatrix.lastFeature(trackIdFeat5) == f3_incoming);
}
int main(int argc, char **argv)