diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp index 680e88132be2b1444fa7e48ec5011c0718a3c019..ce22608d78283e35f796df52d7e5fea1fb00eb6c 100644 --- a/src/processor/processor_landmark_external.cpp +++ b/src/processor/processor_landmark_external.cpp @@ -126,12 +126,13 @@ unsigned int ProcessorLandmarkExternal::processKnown() // Quaterniond(Vector4d(pose_last.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // feat_lmk_last->getMeasurement().head<3>()))); - - auto last_global_meas = - pose_last.at('P') + - Rotation2Dd(pose_last.at('O')(0)) * - (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>()); - WOLF_DEBUG("global meas last: ", last_global_meas.transpose()); + // WOLF_DEBUG("pose_last: ", pose_last); + // WOLF_DEBUG("pose_sen: ", pose_sen); + // auto last_global_meas = + // pose_last.at('P') + + // Rotation2Dd(pose_last.at('O')(0)) * + // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>()); + // WOLF_DEBUG("Last global meas: ", last_global_meas.transpose()); // First we try to match by EXTERNAL_ID if (feat_lmk_last->getExternalId() != -1) @@ -141,7 +142,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() { auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); - WOLF_DEBUG("Feature incoming candidate: ", + WOLF_DEBUG("Feature incoming candidate (by ID): ", feat_lmk_incoming->id(), " - ID: ", feat_lmk_incoming->getExternalId(), @@ -161,11 +162,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() // Quaterniond(Vector4d(pose_incoming.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // feat_lmk_incoming->getMeasurement().head<3>()))); - auto global_meas = - pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * - (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * - feat_lmk_incoming->getMeasurement().head<2>()); - WOLF_DEBUG(" - global meas: ", global_meas.transpose()); + // auto global_meas = + // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * + // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * + // feat_lmk_incoming->getMeasurement().head<2>()); + // WOLF_DEBUG("pose_incoming: ", pose_incoming); + // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Same EXTERNAL_ID @@ -215,7 +217,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() while (feature_incoming_it != new_features_incoming_.end()) { auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); - WOLF_DEBUG("Feature incoming candidate: ", + WOLF_DEBUG("Feature incoming candidate (by TYPE): ", feat_lmk_incoming->id(), " - ID: ", feat_lmk_incoming->getExternalId(), @@ -235,11 +237,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() // Quaterniond(Vector4d(pose_incoming.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // feat_lmk_incoming->getMeasurement().head<3>()))); - auto global_meas = - pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * - (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * - feat_lmk_incoming->getMeasurement().head<2>()); - WOLF_DEBUG(" - global meas: ", global_meas.transpose()); + // auto global_meas = + // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * + // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * + // feat_lmk_incoming->getMeasurement().head<2>()); + // WOLF_DEBUG("pose_incoming: ", pose_incoming); + // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Compatible EXTERNAL_ID (either not defined or same) @@ -292,7 +295,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() { auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); - WOLF_DEBUG("Feature incoming candidate: ", + WOLF_DEBUG("Feature incoming candidate (by distance): ", feat_lmk_incoming->id(), " - ID: ", feat_lmk_incoming->getExternalId(), @@ -312,11 +315,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() // Quaterniond(Vector4d(pose_incoming.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // feat_lmk_incoming->getMeasurement().head<3>()))); - auto global_meas = - pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * - (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * - feat_lmk_incoming->getMeasurement().head<2>()); - WOLF_DEBUG(" - global meas: ", global_meas.transpose()); + // auto global_meas = + // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * + // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * + // feat_lmk_incoming->getMeasurement().head<2>()); + // WOLF_DEBUG("pose_incoming: ", pose_incoming); + // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Compatible EXTERNAL_ID (either not defined or same) @@ -398,21 +402,6 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1 const VectorComposite& _pose2, const VectorComposite& _pose_sen) const { - WOLF_DEBUG("ProcessorLandmarkExternal::detectionDistance: feature ", - _ftr1->id(), - " detection: ", - _ftr1->getMeasurement().transpose(), - " - feature ", - _ftr2->id(), - " detection: ", - _ftr2->getMeasurement().transpose(), - " - _pose1: ", - _pose1, - " - _pose2: ", - _pose2, - " - _pose_sen: ", - _pose_sen); - // Any not available info of poses, assume identity if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO")) @@ -426,26 +415,23 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1 { if (getProblem()->getDim() == 2) { - // auto pose_s1 = SE2::compose(_pose1, _pose_sen); - // auto pose_s2 = SE2::compose(_pose2, _pose_sen); - auto p1 = _pose1.at('P') + - Rotation2Dd(_pose1.at('O')(0)) * - (_pose_sen.at('P') + Rotation2Dd(_pose_sen.at('O')(0)) * _ftr1->getMeasurement().head<2>()); - auto p2 = _pose2.at('P') + - Rotation2Dd(_pose2.at('O')(0)) * - (_pose_sen.at('P') + Rotation2Dd(_pose_sen.at('O')(0)) * _ftr2->getMeasurement().head<2>()); - - WOLF_DEBUG("p1: ", p1.transpose(), " p2: ", p2.transpose(), " - norm: ", (p1 - p2).norm()); + VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen); + VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen); + + Eigen::Vector2d p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>(); + Eigen::Vector2d p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>(); + return (p1 - p2).norm(); } else { - auto pose_s1 = SE3::compose(_pose1, _pose_sen); - auto pose_s2 = SE3::compose(_pose2, _pose_sen); - auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>(); - auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>(); + VectorComposite pose_s1 = SE3::compose(_pose1, _pose_sen); + VectorComposite pose_s2 = SE3::compose(_pose2, _pose_sen); + Eigen::Vector3d p1 = + pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>(); + Eigen::Vector3d p2 = + pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>(); - WOLF_DEBUG("p1: ", p1.transpose(), " p2: ", p2.transpose(), " - norm: ", (p1 - p2).norm()); return (p1 - p2).norm(); } } diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp index 89fb825fed8e04232a341bb38f6b75513166f7b9..3fe0eeef4e0ce85bbae13105bf55fe11cb324482 100644 --- a/test/gtest_processor_landmark_external.cpp +++ b/test/gtest_processor_landmark_external.cpp @@ -93,6 +93,12 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, double _time_span, bool _init_landmarks) { + // INCOMPATIBLE OPTIONS + if (_init_landmarks and mode == 2) + throw std::runtime_error("Landmarks initialized with mode 2 (no id no type), impossible to close loops"); + if (_init_landmarks and mode == 4) + throw std::runtime_error("Landmarks initialized with mode 4 (changing), impossible to close loops"); + dim = _dim; orientation = _orientation; mode = _mode; @@ -172,10 +178,11 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, // Emplace 3 random landmarks for (auto i = 0; i < 3; i++) { + bool init_landmark = _init_landmarks and mode != 3 and i % 3 != 2; LandmarkExternalPtr lmk; if (dim == 2) lmk = LandmarkBase::emplace<LandmarkExternal>( - _init_landmarks ? problem->getMap() : nullptr, + init_landmark ? problem->getMap() : nullptr, i + 1, 3*i + 10, std::make_shared<StatePoint2d>(Vector2d::Random() * 10), @@ -183,7 +190,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, else lmk = LandmarkBase::emplace<LandmarkExternal>( - _init_landmarks ? problem->getMap() : nullptr, + init_landmark ? problem->getMap() : nullptr, i + 1, 3*i + 10, std::make_shared<StatePoint3d>(Vector3d::Random() * 10), @@ -410,7 +417,8 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim, for (auto lmk_map : problem->getMap()->getLandmarkList()) { auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map); - assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1)); + if (lmk_ext->getExternalId() != -1) + assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1)); } } @@ -478,53 +486,65 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_mixed) true); // bool init_landmarks & loop closure } -// TEST_F(ProcessorLandmarkExternalTest, P_2d_id) -// { -// testConfiguration(2, // int dim -// false, // bool orientation -// 0, // int mode -// 0, // double quality_th -// 1e6, // double dist_th -// 2, // int track_length -// 4.5 * dt, // double time_span -// false); // bool init_landmarks & loop closure -// } +TEST_F(ProcessorLandmarkExternalTest, P_2d_id) +{ + testConfiguration(2, // int dim + false, // bool orientation + 0, // int mode + 0, // double quality_th + 1e6, // double dist_th + 2, // int track_length + 4.5 * dt, // double time_span + false); // bool init_landmarks & loop closure +} -// TEST_F(ProcessorLandmarkExternalTest, P_2d_type) -// { -// testConfiguration(2, // int dim -// false, // bool orientation -// 1, // int mode -// 0, // double quality_th -// 1e6, // double dist_th -// 2, // int track_length -// 4.5 * dt, // double time_span -// false); // bool init_landmarks & loop closure -// } +TEST_F(ProcessorLandmarkExternalTest, P_2d_type) +{ + testConfiguration(2, // int dim + false, // bool orientation + 1, // int mode + 0, // double quality_th + 1e6, // double dist_th + 2, // int track_length + 4.5 * dt, // double time_span + false); // bool init_landmarks & loop closure +} -// TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing) -// { -// testConfiguration(2, // int dim -// false, // bool orientation -// 2, // int mode -// 0, // double quality_th -// 1e6, // double dist_th -// 2, // int track_length -// 4.5 * dt, // double time_span -// false); // bool init_landmarks & loop closure -// } +TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing) +{ + testConfiguration(2, // int dim + false, // bool orientation + 2, // int mode + 0, // double quality_th + 1e6, // double dist_th + 2, // int track_length + 4.5 * dt, // double time_span + false); // bool init_landmarks & loop closure +} -// TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed) -// { -// testConfiguration(2, // int dim -// false, // bool orientation -// 3, // int mode -// 0, // double quality_th -// 1e6, // double dist_th -// 2, // int track_length -// 4.5 * dt, // double time_span -// false); // bool init_landmarks & loop closure -// } +TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed) +{ + testConfiguration(2, // int dim + false, // bool orientation + 3, // int mode + 0, // double quality_th + 1e6, // double dist_th + 2, // int track_length + 4.5 * dt, // double time_span + false); // bool init_landmarks & loop closure +} + +TEST_F(ProcessorLandmarkExternalTest, P_2d_changing) +{ + testConfiguration(2, // int dim + false, // bool orientation + 4, // int mode + 0, // double quality_th + 1e6, // double dist_th + 2, // int track_length + 4.5 * dt, // double time_span + false); // bool init_landmarks & loop closure +} // TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id) // {