diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp index 40563fb9b8a0d130c9dd8ef287b379d2e6a73853..af488eb4808f92b2b3a53754c340c1ff97e8bd2f 100644 --- a/test/gtest_processor_tracker_landmark_object.cpp +++ b/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)