processor implementation

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);
 

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");
 

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));
 }
 

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);

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()