Tests refactored

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)