Change gtest for matchingRANSAC

test/gtest_processor_tracker_landmark_object.cpp

@@ -321,18 +321,24 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
     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_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_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);
@@ -341,25 +347,37 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
     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);
 
@@ -373,17 +391,17 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
     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(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)
@@ -393,10 +411,10 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
     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_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_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();
@@ -409,10 +427,9 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
     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;
@@ -421,6 +438,7 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
     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));