Skip to content
Snippets Groups Projects

Resolve "ProcessorLandmarkExternal add class and id not mandatory"

Merged Resolve "ProcessorLandmarkExternal add class and id not mandatory"
490-processorlandmarkexternal-add-class-and-id-not-mandatory-2 into devel
Merged Joan Vallvé Navarro requested to merge 490-processorlandmarkexternal-add-class-and-id-not-mandatory-2 into devel
Viewing commit 93e6a5e4
Prev Next
Show latest version
2 files
+ 107
− 101
Compare changes
  • Side-by-side
  • Inline
Files
2
src/processor/processor_landmark_external.cpp
+ 40
− 54
@@ -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();
}
}