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
This commit is part of merge request !470. Comments created here will be created in the context of that merge request.
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()
// 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();
}
}
......
test/gtest_processor_landmark_external.cpp
+ 67
− 47
View file @ 93e6a5e4
......@@ -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)
// {
......
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