Resolve "Subscriber&processor for landmark external detections"

Closes #478 (closed)

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