From 8ac94b73f08c0aa6b08c706ba552ab081f0f6694 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Tue, 27 Sep 2022 15:40:00 +0200
Subject: [PATCH] set id and add orientation
---
...ssor_tracker_feature_landmark_external.cpp | 24 +++++++++++++++++++
1 file changed, 24 insertions(+)
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index 8502c87e1..e2dd598ea 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -151,6 +151,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
"LandmarkRelativePosition2d",
std::make_shared<StatePoint2d>(lmk_p),
nullptr);
+ lmk->setId(_feature_ptr->landmarkId());
}
// emplace factor
@@ -178,6 +179,17 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
"LandmarkRelativePose2d",
std::make_shared<StatePoint2d>(lmk_p),
std::make_shared<StateAngle>(lmk_o));
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+ // no orientation, add it
+ else if (not lmk->getO())
+ {
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
+
+ double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
+
+ lmk->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
}
// emplace factor
@@ -206,6 +218,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
"LandmarkRelativePosition3d",
std::make_shared<StatePoint3d>(lmk_p),
nullptr);
+ lmk->setId(_feature_ptr->landmarkId());
}
// emplace factor
@@ -233,6 +246,17 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
"LandmarkRelativePose3d",
std::make_shared<StatePoint3d>(lmk_p),
std::make_shared<StateQuaternion>(lmk_o));
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+ // no orientation, add it
+ else if (not lmk->getO())
+ {
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
+
+ lmk->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
}
// emplace factor
--
GitLab