Change function matchingRANSAC

test/gtest_processor_tracker_landmark_object.cpp

@@ -67,7 +67,7 @@ WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject_Wrapper);
  * The class ProcessorTrackerLandmarkObject is sometimes tested via the wrapper ProcessorTrackerLandmarkObject_Wrapper
  */
 // Use the following in case you want to initialize tests with predefined variables or methods.
-class ProcessorTrackerLandmarkObject_class : public testing::Test{
+class ProcessorTrackerLandmarkObject_fixture : public testing::Test{
     public:
         void SetUp() override
         {
@@ -125,7 +125,7 @@ TEST(ProcessorTrackerLandmarkObject, Constructor)
 //     // TODO: GTEST IMPLEMENTATION IS WRONG
 //     //////////////////////////////////////
 //     //////////////////////////////////////
-// TEST_F(ProcessorTrackerLandmarkObject_class, voteForKeyFrame)
+// TEST_F(ProcessorTrackerLandmarkObject_fixture, voteForKeyFrame)
 // {
 
 //     double min_time_vote = prc_obj->getMinTimeVote();
@@ -175,7 +175,7 @@ TEST(ProcessorTrackerLandmarkObject, Constructor)
 // !!!!!!!!!!!!!!!!!!!!!!!
 // not usefull principle?
 // !!!!!!!!!!!!!!!!!!!!!!!
-// TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeaturesDuplicated)
+// TEST_F(ProcessorTrackerLandmarkObject_fixture, detectNewFeaturesDuplicated)
 // {
 //     // No detected features
 //     FeatureBasePtrList features_out;
@@ -217,7 +217,7 @@ TEST(ProcessorTrackerLandmarkObject, Constructor)
 //     ASSERT_EQ(features_out.size(), 1); // detectNewFeatures should keep only one in the final list of new detected features
 // }
 
-TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, detectNewFeatures)
 {
     // No detected features
     FeatureBasePtrList features_out;
@@ -231,8 +231,6 @@ TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
     Eigen::Quaterniond quat;
     Eigen::Vector7d pose;
     Eigen::Matrix6d meas_cov = Matrix6d::Identity();
-    // Eigen::Matrix3d intrinsic = Matrix3d::Identity();
-    // TimeStamp t;
     std::string object_type;
 
     // feature 0
@@ -259,10 +257,10 @@ TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
     object_type = "type2";
     FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
 
-    //we add different features in the list
+    // we add 2 features in the detection list
     features_in.push_back(f0);
     features_in.push_back(f1);
-    //these features are set as the predetected features in last to processing an image
+    // these features are set as the predetected features in last to processing an image
     prc_obj->setLastDetections(features_in);
     // at this point we have 0 detections in last, 2 detections in predetected features with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
     prc_obj->detectNewFeatures(2, C1, features_out);
@@ -287,7 +285,7 @@ TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
     // ASSERT_EQ(std::static_pointer_cast<FeatureObject>(features_out.front())->getObjectType(), "type2");  // TOFIX -> SegFault
 }
 
-TEST_F(ProcessorTrackerLandmarkObject_class, emplaceLandmark)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceLandmark)
 {
     Vector7d pose_landmark((Vector7d()<<0,0,0,0,0,0,1).finished());
     FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, pose_landmark, Matrix6d::Identity(), "thetype");
@@ -299,7 +297,7 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceLandmark)
     ASSERT_MATRIX_APPROX(lmk_object->getState().vector("PO"), pose_landmark, 1e-6);
 }
 
-TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
 {
     FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), "thetype");
 
@@ -312,7 +310,7 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
     ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
 }
 
-TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
+TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
 {
     std::vector<Eigen::Isometry3d> cl_M_o_vec; 
     std::vector<Eigen::Isometry3d> ci_M_o_vec; 
@@ -401,11 +399,10 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
     //Detect all outliers of our batch
     ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model);
 
-    Quaterniond quat_cam;
-    Eigen::Matrix3d wRf = cl_M_ci.linear();
-    quat_cam.coeffs() = R2q(wRf).coeffs().transpose();
+    Quaterniond quat_cam(cl_M_ci.linear());
     Vector3d pos_cam = cl_M_ci.translation();
 
+<<<<<<< HEAD
     Quaterniond quat_cam_RANSAC;
     Eigen::Matrix3d wRf_R = best_model.linear();
     quat_cam_RANSAC.coeffs() = R2q(wRf_R).coeffs().transpose();
@@ -425,6 +422,17 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
     ASSERT_TRUE(outliers_idx.size() == 2);
     ASSERT_TRUE(outliers_idx[0] == 2);
     ASSERT_TRUE(outliers_idx[1] == 4);
+=======
+    Quaterniond quat_cam_RANSAC(outliers.second.linear());
+    Vector3d pos_cam_RANSAC = outliers.second.translation();
+
+    ASSERT_MATRIX_APPROX(pos_cam, pos_cam_RANSAC, 1e-6)
+    ASSERT_MATRIX_APPROX(quat_cam.coeffs(), quat_cam_RANSAC.coeffs(), 1e-6)
+
+    ASSERT_TRUE(outliers.first.size() == 2);
+    ASSERT_TRUE(outliers.first[0] == 2);
+    ASSERT_TRUE(outliers.first[1] == 4);
+>>>>>>> cf5c29b8a52806842bc3265580ac6f701156dba8
 }
 
 TEST(ProcessorTrackerLandmarkObject, isInliers)