Skip to content
Snippets Groups Projects
Commit 93e6a5e4 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

wip not working yet

parent 9fb13bec
No related branches found
No related tags found
1 merge request!470Resolve "ProcessorLandmarkExternal add class and id not mandatory"
Pipeline #17511 failed
Stage: license
    Stage: build_and_test
      Stage: deploy_plugins
        Stage: final_plugins
          Stage: deploy_ros
            Hide whitespace changes
            Inline Side-by-side
            src/processor/processor_landmark_external.cpp
            + 40
            − 54
            View file @ 93e6a5e4
            ...@@ -126,12 +126,13 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -126,12 +126,13 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            // Quaterniond(Vector4d(pose_last.at('O'))) * // Quaterniond(Vector4d(pose_last.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_last->getMeasurement().head<3>()))); // feat_lmk_last->getMeasurement().head<3>())));
            // WOLF_DEBUG("pose_last: ", pose_last);
            auto last_global_meas = // WOLF_DEBUG("pose_sen: ", pose_sen);
            pose_last.at('P') + // auto last_global_meas =
            Rotation2Dd(pose_last.at('O')(0)) * // pose_last.at('P') +
            (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>()); // Rotation2Dd(pose_last.at('O')(0)) *
            WOLF_DEBUG("global meas last: ", last_global_meas.transpose()); // (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 // First we try to match by EXTERNAL_ID
            if (feat_lmk_last->getExternalId() != -1) if (feat_lmk_last->getExternalId() != -1)
            ...@@ -141,7 +142,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -141,7 +142,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            { {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); 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(), feat_lmk_incoming->id(),
            " - ID: ", " - ID: ",
            feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
            ...@@ -161,11 +162,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -161,11 +162,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            // Quaterniond(Vector4d(pose_incoming.at('O'))) * // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>()))); // feat_lmk_incoming->getMeasurement().head<3>())));
            auto global_meas = // auto global_meas =
            pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            feat_lmk_incoming->getMeasurement().head<2>()); // feat_lmk_incoming->getMeasurement().head<2>());
            WOLF_DEBUG(" - global meas: ", global_meas.transpose()); // WOLF_DEBUG("pose_incoming: ", pose_incoming);
            // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
            // MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
            // 1. Same EXTERNAL_ID // 1. Same EXTERNAL_ID
            ...@@ -215,7 +217,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -215,7 +217,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            while (feature_incoming_it != new_features_incoming_.end()) while (feature_incoming_it != new_features_incoming_.end())
            { {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); 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(), feat_lmk_incoming->id(),
            " - ID: ", " - ID: ",
            feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
            ...@@ -235,11 +237,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -235,11 +237,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            // Quaterniond(Vector4d(pose_incoming.at('O'))) * // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>()))); // feat_lmk_incoming->getMeasurement().head<3>())));
            auto global_meas = // auto global_meas =
            pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            feat_lmk_incoming->getMeasurement().head<2>()); // feat_lmk_incoming->getMeasurement().head<2>());
            WOLF_DEBUG(" - global meas: ", global_meas.transpose()); // WOLF_DEBUG("pose_incoming: ", pose_incoming);
            // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
            // MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
            // 1. Compatible EXTERNAL_ID (either not defined or same) // 1. Compatible EXTERNAL_ID (either not defined or same)
            ...@@ -292,7 +295,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -292,7 +295,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            { {
            auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); 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(), feat_lmk_incoming->id(),
            " - ID: ", " - ID: ",
            feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
            ...@@ -312,11 +315,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -312,11 +315,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
            // Quaterniond(Vector4d(pose_incoming.at('O'))) * // Quaterniond(Vector4d(pose_incoming.at('O'))) *
            // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
            // feat_lmk_incoming->getMeasurement().head<3>()))); // feat_lmk_incoming->getMeasurement().head<3>())));
            auto global_meas = // auto global_meas =
            pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) * // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
            (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
            feat_lmk_incoming->getMeasurement().head<2>()); // feat_lmk_incoming->getMeasurement().head<2>());
            WOLF_DEBUG(" - global meas: ", global_meas.transpose()); // WOLF_DEBUG("pose_incoming: ", pose_incoming);
            // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
            // MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
            // 1. Compatible EXTERNAL_ID (either not defined or same) // 1. Compatible EXTERNAL_ID (either not defined or same)
            ...@@ -398,21 +402,6 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1 ...@@ -398,21 +402,6 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
            const VectorComposite& _pose2, const VectorComposite& _pose2,
            const VectorComposite& _pose_sen) const 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 // Any not available info of poses, assume identity
            if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or
            not _pose_sen.includesStructure("PO")) not _pose_sen.includesStructure("PO"))
            ...@@ -426,26 +415,23 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1 ...@@ -426,26 +415,23 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
            { {
            if (getProblem()->getDim() == 2) if (getProblem()->getDim() == 2)
            { {
            // auto pose_s1 = SE2::compose(_pose1, _pose_sen); VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen);
            // auto pose_s2 = SE2::compose(_pose2, _pose_sen); VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen);
            auto p1 = _pose1.at('P') +
            Rotation2Dd(_pose1.at('O')(0)) * Eigen::Vector2d p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
            (_pose_sen.at('P') + Rotation2Dd(_pose_sen.at('O')(0)) * _ftr1->getMeasurement().head<2>()); Eigen::Vector2d p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->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());
            return (p1 - p2).norm(); return (p1 - p2).norm();
            } }
            else else
            { {
            auto pose_s1 = SE3::compose(_pose1, _pose_sen); VectorComposite pose_s1 = SE3::compose(_pose1, _pose_sen);
            auto pose_s2 = SE3::compose(_pose2, _pose_sen); VectorComposite pose_s2 = SE3::compose(_pose2, _pose_sen);
            auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>(); Eigen::Vector3d p1 =
            auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>(); 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(); return (p1 - p2).norm();
            } }
            } }
            ......
            test/gtest_processor_landmark_external.cpp
            + 67
            − 47
            View file @ 93e6a5e4
            ...@@ -93,6 +93,12 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, ...@@ -93,6 +93,12 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
            double _time_span, double _time_span,
            bool _init_landmarks) 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; dim = _dim;
            orientation = _orientation; orientation = _orientation;
            mode = _mode; mode = _mode;
            ...@@ -172,10 +178,11 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, ...@@ -172,10 +178,11 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
            // Emplace 3 random landmarks // Emplace 3 random landmarks
            for (auto i = 0; i < 3; i++) for (auto i = 0; i < 3; i++)
            { {
            bool init_landmark = _init_landmarks and mode != 3 and i % 3 != 2;
            LandmarkExternalPtr lmk; LandmarkExternalPtr lmk;
            if (dim == 2) if (dim == 2)
            lmk = LandmarkBase::emplace<LandmarkExternal>( lmk = LandmarkBase::emplace<LandmarkExternal>(
            _init_landmarks ? problem->getMap() : nullptr, init_landmark ? problem->getMap() : nullptr,
            i + 1, i + 1,
            3*i + 10, 3*i + 10,
            std::make_shared<StatePoint2d>(Vector2d::Random() * 10), std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
            ...@@ -183,7 +190,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, ...@@ -183,7 +190,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
            else else
            lmk = LandmarkBase::emplace<LandmarkExternal>( lmk = LandmarkBase::emplace<LandmarkExternal>(
            _init_landmarks ? problem->getMap() : nullptr, init_landmark ? problem->getMap() : nullptr,
            i + 1, i + 1,
            3*i + 10, 3*i + 10,
            std::make_shared<StatePoint3d>(Vector3d::Random() * 10), std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
            ...@@ -410,7 +417,8 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim, ...@@ -410,7 +417,8 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
            for (auto lmk_map : problem->getMap()->getLandmarkList()) for (auto lmk_map : problem->getMap()->getLandmarkList())
            { {
            auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map); 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) ...@@ -478,53 +486,65 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_mixed)
            true); // bool init_landmarks & loop closure true); // bool init_landmarks & loop closure
            } }
            // TEST_F(ProcessorLandmarkExternalTest, P_2d_id) TEST_F(ProcessorLandmarkExternalTest, P_2d_id)
            // { {
            // testConfiguration(2, // int dim testConfiguration(2, // int dim
            // false, // bool orientation false, // bool orientation
            // 0, // int mode 0, // int mode
            // 0, // double quality_th 0, // double quality_th
            // 1e6, // double dist_th 1e6, // double dist_th
            // 2, // int track_length 2, // int track_length
            // 4.5 * dt, // double time_span 4.5 * dt, // double time_span
            // false); // bool init_landmarks & loop closure false); // bool init_landmarks & loop closure
            // } }
            // TEST_F(ProcessorLandmarkExternalTest, P_2d_type) TEST_F(ProcessorLandmarkExternalTest, P_2d_type)
            // { {
            // testConfiguration(2, // int dim testConfiguration(2, // int dim
            // false, // bool orientation false, // bool orientation
            // 1, // int mode 1, // int mode
            // 0, // double quality_th 0, // double quality_th
            // 1e6, // double dist_th 1e6, // double dist_th
            // 2, // int track_length 2, // int track_length
            // 4.5 * dt, // double time_span 4.5 * dt, // double time_span
            // false); // bool init_landmarks & loop closure false); // bool init_landmarks & loop closure
            // } }
            // TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing) TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing)
            // { {
            // testConfiguration(2, // int dim testConfiguration(2, // int dim
            // false, // bool orientation false, // bool orientation
            // 2, // int mode 2, // int mode
            // 0, // double quality_th 0, // double quality_th
            // 1e6, // double dist_th 1e6, // double dist_th
            // 2, // int track_length 2, // int track_length
            // 4.5 * dt, // double time_span 4.5 * dt, // double time_span
            // false); // bool init_landmarks & loop closure false); // bool init_landmarks & loop closure
            // } }
            // TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed) TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed)
            // { {
            // testConfiguration(2, // int dim testConfiguration(2, // int dim
            // false, // bool orientation false, // bool orientation
            // 3, // int mode 3, // int mode
            // 0, // double quality_th 0, // double quality_th
            // 1e6, // double dist_th 1e6, // double dist_th
            // 2, // int track_length 2, // int track_length
            // 4.5 * dt, // double time_span 4.5 * dt, // double time_span
            // false); // bool init_landmarks & loop closure 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) // TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id)
            // { // {
            ......
            0% or .
            You are about to add 0 people to the discussion. Proceed with caution.
            Finish editing this message first!
            Please register or to comment