ProcessorTrackerLandmark and ProcessorTrackerFeature first working

versions and needed function in map/wolf_problem/node_linked

src/hardware_base.cpp

 #include "hardware_base.h"
-#include "sensor_base.h"
-#include "wolf_problem.h"
 
 HardwareBase::HardwareBase() :
     NodeLinked(MID, "HARDWARE")

src/hardware_base.h

@@ -32,6 +32,10 @@ class HardwareBase : public NodeLinked<WolfProblem, SensorBase>
 
 };
 
+// IMPLEMENTATION
+
+#include "sensor_base.h"
+
 inline void HardwareBase::removeSensor(const SensorBaseIter& _sensor_iter)
 {
     removeDownNode(_sensor_iter);

src/map_base.cpp

 #include "map_base.h"
-#include "wolf_problem.h"
+//#include "wolf_problem.h"
 #include "landmark_base.h"
 
 MapBase::MapBase() :
@@ -19,6 +19,13 @@ void MapBase::addLandmark(LandmarkBase* _landmark_ptr)
     _landmark_ptr->registerNewStateBlocks();
 }
 
+void MapBase::addLandmarkList(LandmarkBaseList _landmark_list)
+{
+    addDownNodeList(_landmark_list);
+    for (auto landmark_ptr : _landmark_list)
+        landmark_ptr->registerNewStateBlocks();
+}
+
 void MapBase::removeLandmark(LandmarkBase* _landmark_ptr)
 {
     removeDownNode(_landmark_ptr->nodeId());

src/map_base.h

@@ -30,6 +30,12 @@ class MapBase : public NodeLinked<WolfProblem,LandmarkBase>
          **/
         virtual void addLandmark(LandmarkBase* _landmark_ptr);
 
+        /** \brief Adds a landmark
+         *
+         * Adds a landmark to the Map. It also updates the lists of StateBlocks that are used by the solver.
+         **/
+        virtual void addLandmarkList(LandmarkBaseList _landmark_list);
+
         void removeLandmark(const LandmarkBaseIter& _landmark_iter);
         void removeLandmark(LandmarkBase* _landmark_ptr);
 

src/node_linked.h

@@ -104,7 +104,7 @@ class NodeLinked : public NodeBase
 
         /** \brief Append a list of new down nodes
          */
-        void appendDownNodeList(LowerNodeList _new_down_node_list);
+        void addDownNodeList(LowerNodeList _new_down_node_list);
 
         /** \brief Gets a reference to down node list
          */
@@ -276,7 +276,7 @@ inline void NodeLinked<UpperType, LowerType>::addDownNode(LowerNodePtr _ptr)
     //std::cout << "node: " << _ptr->nodeId() << " linked to " <<_ptr->upperNodePtr()->nodeId() << std::endl;
 }
 template<class UpperType, class LowerType>
-void NodeLinked<UpperType, LowerType>::appendDownNodeList(LowerNodeList _new_down_node_list)
+void NodeLinked<UpperType, LowerType>::addDownNodeList(LowerNodeList _new_down_node_list)
 {
     assert(!isBottom() && "Trying to add a down node to a bottom node");
     down_node_list_.splice(down_node_list_.end(), _new_down_node_list);

src/processor_tracker_feature.cpp

@@ -18,13 +18,20 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
 
 unsigned int ProcessorTrackerFeature::processKnown()
 {
-    assert(incoming_2_last_.size() == 0 && "In ProcessorTrackerFeature::processKnown(): incoming_2_last_ must be empty before processKnown()");
+    // Compose correspondences to get incoming_2_origin
+    for (auto incoming_feature : *(incoming_ptr_->getFeatureListPtr()))
+        incoming_2_last_[incoming_feature].feature_ptr_ = last_2_origin_[incoming_2_last_[incoming_feature].feature_ptr_].feature_ptr_;
+
+    // the previous incoming_ is now last_
+    last_2_origin_ = incoming_2_last_;
+    // new incoming doesn't have correspondences yet
+    incoming_2_last_.clear();
+
     assert(incoming_ptr_->getFeatureListPtr()->size() == 0 && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
 
     // Track features from last_ptr_ to incoming_ptr_
     FeatureBaseList known_features_list_incoming;
-    unsigned int tracked_features = trackFeatures(*(last_ptr_->getFeatureListPtr()), known_features_list_incoming,
-                                                  incoming_2_last_);
+    trackFeatures(*(last_ptr_->getFeatureListPtr()), known_features_list_incoming, incoming_2_last_);
 
     // Check/correct incoming-origin correspondences
     for (auto known_incoming_feature : known_features_list_incoming)
@@ -37,7 +44,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
         }
 
     // Append not destructed incoming features
-    incoming_ptr_->appendDownNodeList(known_features_list_incoming);
+    incoming_ptr_->addDownNodeList(known_features_list_incoming);
 
     return known_features_list_incoming.size();
 }
@@ -59,23 +66,9 @@ unsigned int ProcessorTrackerFeature::processNew()
     trackFeatures(new_features_list_last_, new_features_list_incoming_, incoming_2_last_);
 
     // Append all new Features to the Capture's list of Features
-    last_ptr_->appendDownNodeList(new_features_list_last_); //TODO JV: is it really necessary to add all new features instead of only the tracked ones? It's easier and probably faster.
-    incoming_ptr_->appendDownNodeList(new_features_list_incoming_);
+    last_ptr_->addDownNodeList(new_features_list_last_); //TODO JV: is it really necessary to add all new features instead of only the tracked ones? It's easier and probably faster.
+    incoming_ptr_->addDownNodeList(new_features_list_incoming_);
 
     // return the number of new features detected in \b last
     return n;
 }
-
-void ProcessorTrackerFeature::advance()
-{
-    // Compose correspondences to get incoming_2_origin
-    for (auto incoming_feature : *(incoming_ptr_->getFeatureListPtr()))
-        incoming_2_last_[incoming_feature].feature_ptr_ =
-                last_2_origin_[incoming_2_last_[incoming_feature].feature_ptr_].feature_ptr_;
-    // incoming_ is going to be last_
-    last_2_origin_ = incoming_2_last_;
-    // new incoming doesn't have correspondences yet
-    incoming_2_last_.clear();
-    // advance tracker
-    ProcessorTracker::advance();
-}

src/processor_tracker_feature.h

@@ -83,13 +83,6 @@ class ProcessorTrackerFeature : public ProcessorTracker
         FeatureCorrespondenceMap incoming_2_last_;
         FeatureCorrespondenceMap last_2_origin_;
 
-        /** \brief Advance the incoming Capture to become the last.
-         *
-         * Call this when the tracking and keyframe policy work is done and
-         * we need to get ready to accept a new incoming Capture.
-         */
-        virtual void advance();
-
         /** \brief Tracker function
          * \return The number of successful tracks.
          *

src/processor_tracker_landmark.cpp

@@ -19,3 +19,50 @@ ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
     // TODO Auto-generated destructor stub
 }
 
+unsigned int ProcessorTrackerLandmark::processNew()
+{
+    /* Rationale: A keyFrame will be created using the last Capture.
+     * First, we work on this Capture to detect new Features,
+     * eventually create Landmarks with them,
+     * and in such case create the new Constraints feature-landmark.
+     * When done, we need to track these new Features to the incoming Capture.
+     * At the end, all new Features are appended to the lists of known Features in
+     * the last and incoming Captures.
+     */
+    // We first need to populate the \b last Capture with new Features
+    unsigned int n = detectNewFeatures();
+    LandmarkBaseList new_landmarks;
+    for (auto new_feature_ptr : new_features_list_last_)
+    {
+        // create new landmark
+        LandmarkBase* new_lmk_ptr = createLandmark(new_feature_ptr);
+        new_landmarks.push_back(new_lmk_ptr);
+        // create new correspondence
+        incoming_2_landmark_[new_feature_ptr] = LandmarkMatch(new_lmk_ptr, 1); // max score
+    }
+    // Find the new landmarks in incoming_ptr_
+    findLandmarks(new_landmarks, new_features_list_incoming_, incoming_2_landmark_);
+    // Append all new Features to the Capture's list of Features
+    last_ptr_->addDownNodeList(new_features_list_last_);
+    incoming_ptr_->addDownNodeList(new_features_list_incoming_);
+    // Append new landmarks to the map
+    getWolfProblem()->addLandmarkList(new_landmarks);
+    // return the number of new features detected in \b last
+    return n;
+}
+
+unsigned int ProcessorTrackerLandmark::processKnown()
+{
+    // the previous incoming_ is now last_
+    last_2_landmark_ = incoming_2_landmark_;
+    // new incoming doesn't have correspondences yet
+    incoming_2_landmark_.clear();
+
+    // Find landmarks in incoming_ptr_
+    FeatureBaseList known_features_list_incoming;
+    unsigned int found_landmarks = findLandmarks(*(getWolfProblem()->getMapPtr()->getLandmarkListPtr()),
+                                                 known_features_list_incoming, incoming_2_landmark_);
+    // Append found incoming features
+    incoming_ptr_->appendDownNodeList(known_features_list_incoming);
+    return found_landmarks;
+}

src/processor_tracker_landmark.h

@@ -22,8 +22,8 @@ struct LandmarkMatch
         {
 
         }
-        LandmarkMatch(FeatureBase* _incoming_feature_ptr, const WolfScalar& _normalized_score) :
-                landmark_ptr_(_incoming_feature_ptr), normalized_score_(_normalized_score)
+        LandmarkMatch(LandmarkBase* _landmark_ptr, const WolfScalar& _normalized_score) :
+                landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
         {
 
         }
@@ -62,24 +62,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          * It must also generate the constraints, of the correct type, derived from ConstraintBase
          * (through ConstraintAnalytic or ConstraintSparse).
          */
-        virtual unsigned int processKnown()
-        {
-            // Find landmarks in incoming_ptr_
-            FeatureBaseList feature_incoming_known;
-            unsigned int found_landmarks = findLandmarks(*(getWolfProblem()->getMapPtr()->getLandmarkListPtr()),
-                                                         feature_incoming_known, incoming_2_landmark);
-
-            // Check/correct incoming-origin correspondences
-            for (auto incoming_feature : feature_incoming_known)
-            {
-                // 1. Add tracked feature to incoming capture
-                incoming_ptr_->addFeature(incoming_feature);
-
-                // 2. create and add constraint
-                incoming_feature->addConstraint(createConstraint(incoming_feature, incoming_2_landmark[incoming_feature].landmark_ptr_));
-            }
-            return found_landmarks;
-        }
+        virtual unsigned int processKnown();
 
         /** \brief Find provided landmarks in the incoming capture
          * \param _landmark_list_in input list of landmarks to be found in incoming
@@ -106,52 +89,6 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          */
         virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr) = 0;
 
-        unsigned int processNew()
-        {
-            /* Rationale: A keyFrame will be created using the last Capture.
-             * First, we work on this Capture to detect new Features,
-             * eventually create Landmarks with them,
-             * and in such case create the new Constraints feature-landmark.
-             * When done, we need to track these new Features to the incoming Capture.
-             * At the end, all new Features are appended to the lists of known Features in
-             * the last and incoming Captures.
-             */
-            // We first need to populate the \b last Capture with new Features
-            unsigned int n = detectNewFeatures();
-            // TODO: class member?
-            LandmarkBaseList new_landmarks;
-            for (auto feature_ptr : new_features_list_last_)
-            {
-                // create new landmark
-                LandmarkBase* new_lmk_ptr = createLandmark(feature_ptr);
-                new_landmarks.push_back(new_lmk_ptr);
-                // create and add constraint
-                feature_ptr->addConstraint(createConstraint(feature_ptr, new_lmk_ptr));
-            }
-
-            // Find new landmarks in incoming_ptr_
-            FeatureLandmarkMap incoming_2_landmark;
-            findLandmarks(*(getWolfProblem()->getMapPtr()->getLandmarkListPtr()), new_features_list_incoming_, incoming_2_landmark);
-            for (auto new_incoming_feature : new_features_list_incoming_)
-            {
-                // 1. Add tracked feature to incoming capture
-                incoming_ptr_->addFeature(new_incoming_feature);
-
-                // 2. create and add constraint
-                new_incoming_feature->addConstraint(createConstraint(new_incoming_feature, incoming_2_landmark[new_incoming_feature].landmark_ptr_));
-            }
-
-            // Append all new Features to the Capture's list of Features
-            last_ptr_->getFeatureListPtr()->splice(last_ptr_->getFeatureListPtr()->end(), new_features_list_last_);
-            incoming_ptr_->getFeatureListPtr()->splice(incoming_ptr_->getFeatureListPtr()->end(),
-                                                       new_features_list_incoming_);
-
-            // TODO: append new landmarks
-            //getWolfProblem()->addLandmark();
-
-            // return the number of new features detected in \b last
-            return n;
-        }
+        unsigned int processNew();
 };
-
 #endif /* PROCESSOR_TRACKER_LANDMARK_H_ */

src/wolf_problem.cpp

@@ -8,8 +8,7 @@
 
 WolfProblem::WolfProblem(FrameStructure _frame_structure) :
         NodeBase("WOLF_PROBLEM"), //
-        location_(TOP), trajectory_ptr_(new TrajectoryBase(_frame_structure)), map_ptr_(new MapBase), hardware_ptr_(
-                new HardwareBase)
+        location_(TOP), trajectory_ptr_(new TrajectoryBase(_frame_structure)), map_ptr_(new MapBase), hardware_ptr_(new HardwareBase)
 {
     trajectory_ptr_->linkToUpperNode(this);
     map_ptr_->linkToUpperNode(this);
@@ -115,16 +114,25 @@ FrameBase* WolfProblem::createFrame(FrameType _frame_type, const Eigen::VectorXs
     return trajectory_ptr_->getLastFramePtr();
 }
 
+bool WolfProblem::permitKeyFrame(ProcessorBase* _processor_ptr)
+{
+    return true;
+}
+
 void WolfProblem::addLandmark(LandmarkBase* _lmk_ptr)
 {
     getMapPtr()->addLandmark(_lmk_ptr);
 }
 
+void WolfProblem::addLandmarkList(LandmarkBaseList _lmk_list)
+{
+    getMapPtr()->addLandmarkList(_lmk_list);
+}
+
 void WolfProblem::addStateBlockPtr(StateBlock* _state_ptr)
 {
     // add the state unit to the list
     state_block_ptr_list_.push_back(_state_ptr);
-
     // queue for solver manager
     state_block_add_list_.push_back(_state_ptr);
 }
@@ -139,7 +147,6 @@ void WolfProblem::removeStateBlockPtr(StateBlock* _state_ptr)
 {
     // add the state unit to the list
     state_block_ptr_list_.remove(_state_ptr);
-
     // queue for solver manager
     state_block_remove_list_.push_back(_state_ptr->getPtr());
 }
@@ -213,13 +220,37 @@ void WolfProblem::addTrajectory(TrajectoryBase* _trajectory_ptr)
     trajectory_ptr_->linkToUpperNode(this);
 }
 
+MapBase* WolfProblem::getMapPtr()
+{
+    return map_ptr_;
+}
+
+TrajectoryBase* WolfProblem::getTrajectoryPtr()
+{
+    return trajectory_ptr_;
+}
+
+HardwareBase* WolfProblem::getHardwarePtr()
+{
+    return hardware_ptr_;
+}
+
 FrameBase* WolfProblem::getLastFramePtr()
 {
     return trajectory_ptr_->getLastFramePtr();
 }
 
-void WolfProblem::removeDownNode(const LowerNodePtr _ptr)
+StateBlockList* WolfProblem::getStateListPtr()
 {
-    //
+    return &state_block_ptr_list_;
 }
 
+std::list<StateBlock*>* WolfProblem::getStateBlockAddList()
+{
+    return &state_block_add_list_;
+}
+
+std::list<StateBlock*>* WolfProblem::getStateBlockUpdateList()
+{
+    return &state_block_update_list_;
+}

src/wolf_problem.h

@@ -82,6 +82,8 @@ class WolfProblem : public NodeBase
 
         void addLandmark(LandmarkBase* _lmk_ptr);
 
+        void addLandmarkList(LandmarkBaseList _lmk_list);
+
         /** \brief Adds a new state block to be added to solver manager
          */
         void addStateBlockPtr(StateBlock* _state_ptr);
@@ -186,45 +188,10 @@ class WolfProblem : public NodeBase
          *
          * This empty function is needed by the destruct() node_linked function.
          */
-        void removeDownNode(const LowerNodePtr _ptr);
+        void removeDownNode(const LowerNodePtr _ptr){};
 
 };
 
-inline bool WolfProblem::permitKeyFrame(ProcessorBase* _processor_ptr)
-{
-    return true;
-}
-
-inline MapBase* WolfProblem::getMapPtr()
-{
-    return map_ptr_;
-}
-
-inline TrajectoryBase* WolfProblem::getTrajectoryPtr()
-{
-    return trajectory_ptr_;
-}
-
-inline HardwareBase* WolfProblem::getHardwarePtr()
-{
-    return hardware_ptr_;
-}
-
-inline StateBlockList* WolfProblem::getStateListPtr()
-{
-    return &state_block_ptr_list_;
-}
-
-inline std::list<StateBlock*>* WolfProblem::getStateBlockAddList()
-{
-    return &state_block_add_list_;
-}
-
-inline std::list<StateBlock*>* WolfProblem::getStateBlockUpdateList()
-{
-    return &state_block_update_list_;
-}
-
 inline std::list<WolfScalar*>* WolfProblem::getStateBlockRemoveList()
 {
     return &state_block_remove_list_;