diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 566b8b0d6e88fe3e64a2e8cbc0312b720a32d33d..588e6b43917971473f72cddd9701e5b3954760a4 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -115,8 +115,25 @@ public:
prc_obj->setOriginPtr(C1);
prc_obj->setLastPtr(C1);
+ // 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;
+ // Give random values
+ 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;
+
+ // Normalizing quaternions
+ 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_3_.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();
- // define camera and object poses
}
public:
@@ -127,6 +144,17 @@ public:
ProcessorBasePtr prc;
FrameBasePtr F1;
CaptureBasePtr C1;
+
+ // define camera and object poses
+ Vector7d pose_cam_last_;
+ Vector7d pose_cam_incoming_;
+
+ // Create 5 objects
+ Vector7d pose_object_1_;
+ Vector7d pose_object_2_;
+ Vector7d pose_object_3_;
+ Vector7d pose_object_4_;
+ Vector7d pose_object_5_;
};
////////////////////////////////////////////////////////////////
@@ -334,44 +362,15 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
std::vector<std::pair<int, int>> outliers_idx;
Eigen::Isometry3d best_model;
- // Camera poses
- Vector7d pose_cam_last;
- Vector7d pose_cam_incoming;
-
- // Create 5 objects
- Vector7d pose_object_1;
- Vector7d pose_object_2;
- Vector7d pose_object_3;
- Vector7d pose_object_4;
- Vector7d pose_object_5;
-
- // 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;
- 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;
-
- // Normalizing quaternions
- 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();
-
// Transform pose into isometry
- 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_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 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;
@@ -434,34 +433,14 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
ASSERT_TRUE(outliers_idx[1].first == 4);
}
-TEST(ProcessorTrackerLandmarkObject, isInliers)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, isInliers)
{
- // Camera poses
- Vector7d pose_cam_last;
- Vector7d pose_cam_incoming;
-
- // Create 2 objects
- Vector7d pose_object_1;
- Vector7d pose_object_2;
-
- // 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;
- 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;
-
- // Normalizing quaternions
- 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();
-
// Transform pose into isometry
- 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_cl = posevec_to_isometry(pose_cam_last_);
+ Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming_);
- Eigen::Isometry3d cl_M_o1 = posevec_to_isometry(pose_object_1);
- Eigen::Isometry3d cl_M_o2 = posevec_to_isometry(pose_object_2);
+ Eigen::Isometry3d cl_M_o1 = posevec_to_isometry(pose_object_1_);
+ Eigen::Isometry3d cl_M_o2 = posevec_to_isometry(pose_object_2_);
Eigen::Isometry3d cl_M_o3 = Eigen::Isometry3d::Identity();
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
@@ -599,7 +578,7 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
// return flux;
// }
-TEST(ProcessorTrackerLandmarkObject, allMatchesSameType)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, allMatchesSameType)
{
FeatureBasePtrList features_last;
FeatureBasePtrList features_incoming;
@@ -607,64 +586,48 @@ TEST(ProcessorTrackerLandmarkObject, allMatchesSameType)
std::vector<Eigen::Isometry3d> cl_M_o_vec;
std::vector<Eigen::Isometry3d> ci_M_o_vec;
- Eigen::Vector7d pose;
- Eigen::Vector3d pos;
-
-
// feature 0
- pose << 0.8, 2.5, 2.3, 2.5, 1.2, 2.0, 1.1;
- Eigen::Isometry3d cl_M_f0 = appendFeatureLast("type0", pose, features_last);
+ Eigen::Isometry3d cl_M_f1 = appendFeatureLast("type0", pose_object_1_, features_last);
// feature 1
- pose << 0.5, 3.5, 3.2, 1.5, 0.2, 2.1, 1.0;
- Eigen::Isometry3d cl_M_f1 = appendFeatureLast("type0", pose, features_last);
+ Eigen::Isometry3d cl_M_f2 = appendFeatureLast("type0", pose_object_2_, features_last);
// feature 2
- pose << 1.5, 3.2, 1.2, 2.0, 2.2, 1.0, 1.0;
- Eigen::Isometry3d cl_M_f2 = appendFeatureLast("type2", pose, features_last);
+ Eigen::Isometry3d cl_M_f3 = appendFeatureLast("type2", pose_object_3_, features_last);
// feature 3
- pose << 2.1, 1.1, 2.1, 1.0, 1.2, 1.3, 1.1;
- Eigen::Isometry3d cl_M_f3 = appendFeatureLast("type1", pose, features_last);
+ Eigen::Isometry3d cl_M_f4 = appendFeatureLast("type1", pose_object_4_, features_last);
// feature 4
- pose << 1.9, 4.2, 8.2, 3.0, 4.2, 8.0, 1.6;
- Eigen::Isometry3d cl_M_f4 = appendFeatureLast("type1", pose, features_last);
-
- Eigen::Vector7d pose_cam_last;
- Eigen::Vector7d pose_cam_incoming;
- 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_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
- pose_cam_incoming.tail<4>() = pose_cam_incoming.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 cl_M_f5 = appendFeatureLast("type1", pose_object_5_, features_last);
+
+
+ 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 0
- appendFeaturencoming("type0", cl_M_f0, ci_M_cl, features_incoming); // go with f0
+ appendFeaturencoming("type0", cl_M_f1, ci_M_cl, features_incoming); // go with f1
// feature 1
- appendFeaturencoming("type0", cl_M_f1, ci_M_cl, features_incoming); // go with f1
+ appendFeaturencoming("type0", cl_M_f2, ci_M_cl, features_incoming); // go with f2
// feature 2
- appendFeaturencoming("type2", cl_M_f2, ci_M_cl, features_incoming); // go with f2
+ appendFeaturencoming("type2", cl_M_f3, ci_M_cl, features_incoming); // go with f3
// feature 3
- appendFeaturencoming("type0", cl_M_f0, Eigen::Isometry3d::Identity(), features_incoming); //outliers
+ appendFeaturencoming("type0", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 4
- appendFeaturencoming("type1", cl_M_f0, Eigen::Isometry3d::Identity(), features_incoming); //outliers
+ appendFeaturencoming("type1", cl_M_f1, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 5
- appendFeaturencoming("type1", cl_M_f3, ci_M_cl, features_incoming); // go with f3
+ appendFeaturencoming("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
-
auto all_matches = ProcessorTrackerLandmarkObject::createAllMatchesSameType(features_last, features_incoming, cl_M_o_vec, ci_M_o_vec);
// 2 * 3 (type0) + 2 * 3 (type1) + 1 * 1 (type2) = 13
@@ -697,8 +660,8 @@ TEST(ProcessorTrackerLandmarkObject, allMatchesSameType)
ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(0, 0)));
ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(1, 1)));
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, 5)));
- ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 6)));
+ 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)));
}
int main(int argc, char **argv)