diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index ceaad953c6d64f4d7abb2033b5e8217a18fc1475..f5c9ad4632d0c704b73c5273c2d9ba3ee7930d0d 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -31,7 +31,7 @@ struct LandmarkDetection
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
int id; // id of landmark
-}
+};
WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index af09f86002c697ad026ad9942d218026808555c4..da7113fa583bd7838bda5ee9a96c610731814dcd 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -113,7 +113,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
-
+ LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
+ LandmarkBasePtr getLandmark(const unsigned int& _id);
+
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index 32d3dca5ea90141047168b5432b46cf8d102e20c..ebf00489363b359f08e9ec818395948f559623ad 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -31,9 +31,9 @@ CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
{
if (_detections.size() != _covs.size() or _detections.size() != _ids.size())
- throw std::runtime_error("CaptureLandmarksExternal constructor: '_detections', '_covs' and '_ids' should have the same size.")
+ throw std::runtime_error("CaptureLandmarksExternal constructor: '_detections', '_covs' and '_ids' should have the same size.");
- for (auto i = 0; i < _detections.size(); i++)
+ for (auto i = 0; i < _detections.size(); i++)
addDetection(_detections.at(i), _covs.at(i), _ids.at(i));
}
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index b629aba0805d22efc6b23b72e0057ffcc6a30643..0c002934af723fd242babeb22cabd4de15823e9f 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -65,6 +65,36 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
landmark_list_.remove(_landmark_ptr);
}
+LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
+{
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBaseConstPtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
+
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
+
+ return (*lmk_it);
+}
+
+LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
+{
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBasePtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
+
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
+
+ return (*lmk_it);
+}
+
void MapBase::load(const std::string& _map_file_dot_yaml)
{
YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index 94d236810655616f3a5678c43a81bef2f4edef53..8502c87e1ed01fc127919df8773ac90a2ddd7673 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -20,16 +20,26 @@
//
//--------LICENSE_END--------
-#include "processor_tracker_feature_landmark_external.h"
+#include "core/processor/processor_tracker_feature_landmark_external.h"
#include "core/capture/capture_landmarks_external.h"
#include "core/feature/feature_base.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+//#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+
+using namespace Eigen;
namespace wolf
{
void ProcessorTrackerFeatureLandmarkExternal::preProcess()
{
- assert(detections_incoming_.empty())
+ assert(detections_incoming_.empty());
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks)
@@ -42,7 +52,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
"FeatureLandmarkExternal",
detection.measure,
detection.covariance);
- ftr.setLandmarkId(detection.id);
+ ftr->setLandmarkId(detection.id);
detections_incoming_.push_back(ftr);
}
@@ -55,7 +65,7 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
{
WOLF_INFO("tracking " , _features_in.size() , " features...");
- if (_capture != last_ptr and _capture != incoming_ptr)
+ if (_capture != last_ptr_ and _capture != incoming_ptr_)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures unknown capture");
FeatureBasePtrList& landmark_detections = (_capture == last_ptr_ ? detections_last_ : detections_incoming_);
@@ -64,7 +74,7 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
{
for (auto feat_candidate : landmark_detections)
{
- if (feat_candidate->getLandmarkId() == feat_in->getLandmarkId()) // TODO
+ if (feat_candidate->landmarkId() == feat_in->landmarkId())
{
_features_out.push_back(feat_candidate);
_feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
@@ -92,7 +102,7 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const in
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out)
{
- if (_capture != last_ptr and _capture != incoming_ptr)
+ if (_capture != last_ptr_ and _capture != incoming_ptr_)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures unknown capture");
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(_capture);
@@ -118,7 +128,125 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const in
FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- // TODO
+ assert(getProblem());
+ assert(getProblem()->getMap());
+
+ // Get landmark
+ LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(_feature_ptr->landmarkId());
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 2)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 3)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 3)
+ {
+ throw std::runtime_error("FactorRelativePosition3dWithExtrinsics is not implemented yet");
+
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
+ }
+
+ // emplace factor
+ // return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr,
+ // _feature_ptr,
+ // lmk,
+ // shared_from_this(),
+ // params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>());
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ else
+ throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::emplaceFactor unknown case");
+
+ // not reachable
+ return nullptr;
}
void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()