Merge branch '489-multiple-processors-per-sensor-2' into 'devel'

Resolve "multiple processors per sensor"

Closes #489

See merge request !468

include/core/landmark/landmark_base.h

@@ -58,6 +58,7 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
         // Navigate wolf tree
         void setProblem(ProblemPtr _problem) override final;
         unsigned int landmark_id_; ///< landmark unique id
+	    unsigned int track_id_; ///< associated track id
         TimeStamp stamp_;       ///< stamp of the creation of the landmark
         Eigen::VectorXd descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
 
@@ -80,6 +81,9 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
         unsigned int id() const;
         void setId(unsigned int _id);
 
+	    unsigned int trackId(); // get track ID
+	    void setTrackId(unsigned int _track_id); // set track ID
+
         // State blocks
         //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
         //std::vector<StateBlockPtr> getUsedStateBlockVec();
@@ -176,6 +180,18 @@ inline void LandmarkBase::setId(unsigned int _id)
         landmark_id_count_ = _id;
 }
 
+inline unsigned int LandmarkBase::trackId()
+{
+    return track_id_;
+}
+
+inline void LandmarkBase::setTrackId(unsigned int _track_id) 
+{
+    track_id_ = _track_id;
+}
+
+
+
 inline unsigned int LandmarkBase::getHits() const
 {
     return constrained_by_list_.size();

include/core/processor/processor_tracker.h

@@ -142,6 +142,8 @@ class ProcessorTracker : public ProcessorBase
         virtual CaptureBasePtr getOrigin();
         virtual CaptureBaseConstPtr getLast() const;
         virtual CaptureBasePtr getLast();
+        virtual FrameBaseConstPtr getLastFrame() const;
+        virtual FrameBasePtr getLastFrame();
         virtual CaptureBaseConstPtr getIncoming() const;
         virtual CaptureBasePtr getIncoming();
 
@@ -343,6 +345,16 @@ inline CaptureBasePtr ProcessorTracker::getLast()
     return last_ptr_;
 }
 
+inline FrameBaseConstPtr ProcessorTracker::getLastFrame() const
+{
+    return last_frame_ptr_;
+}
+
+inline FrameBasePtr ProcessorTracker::getLastFrame()
+{
+    return last_frame_ptr_;
+}
+
 inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
 {
     return incoming_ptr_;

src/processor/processor_tracker.cpp

@@ -82,10 +82,25 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
 
-            // Append incoming to KF
-            incoming_ptr_->link(keyframe_from_callback);
+            // check if the callback keyframe has a capture of this sensor
+            auto capture_from_callback = keyframe_from_callback->getCaptureOf(this->getSensor());
+
+            if (incoming_ptr_ == capture_from_callback)
+            {
+                // If captures match, then frames must match too
+                assert(incoming_ptr_->getFrame() != nullptr 
+                        and incoming_ptr_->getFrame() == keyframe_from_callback 
+                        and "The keyframe has a Capture from this sensor, but this capture is not incoming!");
+                WOLF_DEBUG("PT ", getName(), " Incoming capture has been processed previously by another processor!")
+            }
+            else
+            {
+                WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed by any other processor!")
+                
+                // Join KF
+                incoming_ptr_->link(keyframe_from_callback);
+            }
 
-            // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
             // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
             processKnown();
@@ -95,6 +110,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Update pointers
             origin_ptr_     = incoming_ptr_;
             last_ptr_       = incoming_ptr_;
+            last_frame_ptr_ = keyframe_from_callback;
             incoming_ptr_   = nullptr;
 
             break;
@@ -103,13 +119,18 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
 
+            // Check if incoming has already a Frame
+            auto frame_incoming = incoming_ptr_->getFrame();
+            assert(frame_incoming == nullptr and " Incoming capture has been processed and linked by another processor, but no keyframe callback was received!");
+
+            WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
+
             // make a new KF at incoming
             FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
-                                                                  incoming_ptr_->getTimeStamp(),
-                                                                  getProblem()->getFrameStructure(),
-                                                                  getProblem()->getState());
-
-            // link incoming to the new KF
+                                                                incoming_ptr_->getTimeStamp(),
+                                                                getProblem()->getFrameStructure(),
+                                                                getProblem()->getState(incoming_ptr_->getTimeStamp()));
+            // Append incoming to KF
             incoming_ptr_->link(keyframe);
 
             // Process info
@@ -126,6 +147,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Update pointers
             origin_ptr_ = incoming_ptr_;
             last_ptr_   = incoming_ptr_;
+            last_frame_ptr_ = keyframe;
             incoming_ptr_ = nullptr;
 
             break;
@@ -154,13 +176,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Make a NON KEY Frame to hold incoming capture
             FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                 getProblem()->getFrameStructure(),
-                                                                getProblem()->getState());
-            incoming_ptr_->link(frame);
+                                                                getProblem()->getState(incoming_ptr_->getTimeStamp()));
 
-            // Process info
-            // If we have known landmarks or features. Process them.
-            if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
-                processKnown();
+            // Process known information
+            processKnown();
             
             // Both Trackers:  We have a last_ Capture with not enough features, so populate it.
             processNew(params_tracker_->max_new_features);
@@ -188,16 +207,29 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             processKnown();
 
-            // Capture last_ is added to the new keyframe
-            FrameBasePtr last_old_frame = last_ptr_->getFrame();
-            last_ptr_->move(keyframe_from_callback);
-            last_old_frame->remove();
+            // chack if the received KF has a capture of this sensor, and if it matches with last_ptr
+            if (last_ptr_ == keyframe_from_callback->getCaptureOf(this->getSensor()))
+            {
+                WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+
+                // If captures match, then frames must match too
+                assert( last_ptr_->getFrame() == keyframe_from_callback 
+                        and "The keyframe has a Capture from this sensor, but this capture is not last!");
+
+                // No need to join KF since it is the same capture, and it was already joined by the other processor
+            }
+            else
+            {
+                WOLF_DEBUG("PT ", getName(), " Last capture had not been processed previously!")
+                
+                // join KF
+                last_ptr_->link(keyframe_from_callback);
+            }
 
             // Create new NON KEY frame for incoming
             FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                            getProblem()->getFrameStructure(),
                                                            getProblem()->getState());
-            incoming_ptr_->link(frame);
 
             // Detect new Features, initialize Landmarks, ...
             processNew(params_tracker_->max_new_features);
@@ -228,21 +260,21 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // We create a KF
                 // set KF on last
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-                last_ptr_->getFrame()->link(getProblem());
+                last_frame_ptr_->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+                last_ptr_->link(last_frame_ptr_);
+                last_frame_ptr_->link(getProblem());
 
                 // Establish factors
                 establishFactors();
 
-                // Call the new keyframe callback in order to let the other processors to establish their factors
-                getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
+                // Call the new keyframe callback in order to let the other processors to join
+                getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
 
 
                 // make NON KEY frame; append incoming to new frame
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
-                incoming_ptr_   ->link(frame);
 
                 // Reset this
                 resetDerived();
@@ -261,8 +293,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
-                incoming_ptr_->link(frame);
-                last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
 
                 // Advance this
                 advanceDerived();
@@ -315,7 +345,7 @@ void ProcessorTracker::computeProcessingStep()
 
             if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
             {
-                if (last_ptr_->getFrame()->getProblem())
+                if (last_frame_ptr_->getProblem())
                 {
                     WOLF_WARN("||*||");
                     WOLF_INFO(" ... It seems you missed something!");
@@ -341,7 +371,7 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
         _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << " Frm"
-                << getLast()->getFrame()->id() << std::endl;
+                << getLastFrame()->id() << std::endl;
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
 }

test/CMakeLists.txt

@@ -225,6 +225,10 @@ wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processo
 wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
 target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC dummy)
 
+# ProcessorTrackerFeatureDummy 2 processors in one sensor test
+wolf_add_gtest(gtest_processor_tracker_two_processors_one_sensor gtest_processor_tracker_two_processors_one_sensor.cpp)
+target_link_libraries(gtest_processor_tracker_two_processors_one_sensor PUBLIC dummy)
+
 # ProcessorTrackerLandmarkDummy class test
 wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
 target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)

test/gtest_processor_landmark_external.cpp

@@ -296,17 +296,17 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
             ASSERT_FALSE(fac_list.empty());
             
             // factors' type
-            if (should_emplace_KF)
-                for (auto fac : fac_list)
+            for (auto fac : fac_list)
+            {
+                if (fac->getProcessor() == processor)
                 {
-                    if (fac->getProcessor() == processor)
-                    {
-                        ASSERT_EQ(fac->getType(), std::string("FactorRelative") + 
-                                                  (orientation ? "Pose" : "Position") + 
-                                                  (dim == 2 ? "2d" : "3d") + 
-                                                  "WithExtrinsics");
-                    }
+                    ASSERT_EQ(fac->getType(), std::string("FactorRelative") + 
+                                                (orientation ? "Pose" : "Position") + 
+                                                (dim == 2 ? "2d" : "3d") + 
+                                                "WithExtrinsics");
                 }
+            }
+
             // landmarks created by processor
             if (not add_landmarks)
             {

test/gtest_processor_tracker_two_processors_one_sensor.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/factory_sensor.h"
+#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummyDummy);
+
+class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
+{
+  public:
+    ProcessorTrackerFeatureDummyDummy(ParamsProcessorTrackerFeatureDummyPtr& _params)
+        : ProcessorTrackerFeatureDummy(_params)
+    {
+    }
+
+    void setLast(CaptureBasePtr _last_ptr)
+    {
+        last_ptr_ = _last_ptr;
+    }
+    void setInc(CaptureBasePtr _incoming_ptr)
+    {
+        incoming_ptr_ = _incoming_ptr;
+    }
+
+    unsigned int callProcessKnown()
+    {
+        return this->processKnown();
+    }
+
+    unsigned int callProcessNew(const int& _max_new_features)
+    {
+        return this->processNew(_max_new_features);
+    }
+
+    unsigned int callDetectNewFeatures(const int&            _max_features,
+                                       const CaptureBasePtr& _capture,
+                                       FeatureBasePtrList&   _features_out)
+    {
+        return this->detectNewFeatures(_max_features, _capture, _features_out);
+    }
+
+    unsigned int callTrackFeatures(const FeatureBasePtrList& _features_in,
+                                   const CaptureBasePtr&     _capture,
+                                   FeatureBasePtrList&       _features_out,
+                                   FeatureMatchMap&          _feature_correspondences)
+    {
+        return this->trackFeatures(_features_in, _capture, _features_out, _feature_correspondences);
+    }
+
+    FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+    {
+        return this->emplaceFactor(_feature_ptr, _feature_other_ptr);
+    }
+
+    void callEstablishFactors()
+    {
+        this->establishFactors();
+    }
+
+    TrackMatrix getTrackMatrix()
+    {
+        return track_matrix_;
+    }
+
+    FeatureMatchMap getMatchesLastFromIncoming()
+    {
+        return matches_last_from_incoming_;
+    }
+
+    void callReset()
+    {
+        this->resetDerived();
+        origin_ptr_   = last_ptr_;
+        last_ptr_     = incoming_ptr_;
+        incoming_ptr_ = nullptr;
+    };
+};
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerFeatureDummyDoubledTest : public testing::Test
+{
+  public:
+    ProblemPtr                            problem;
+    SensorBasePtr                         sensor;
+    ParamsProcessorTrackerFeatureDummyPtr params;
+    ProcessorTrackerFeatureDummyDummyPtr  processor_1, processor_2;
+
+    // ~ProcessorTrackerFeatureDummyDoubledTest() override{}
+
+    void SetUp() override
+    {
+        // Wolf problem
+        problem = Problem::create("PO", 2);
+
+        // Install camera
+        sensor = problem->installSensor("SensorOdom2d",
+                                        "dummy sensor",
+                                        (Eigen::Vector3d() << 0, 0, 0).finished(),
+                                        std::make_shared<ParamsSensorBase>());
+
+        // Install processor
+        params                            = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
+        params->max_new_features          = 10;
+        params->min_features_for_keyframe = 7;
+        params->time_tolerance            = 0.5;
+        params->voting_active             = true;
+        params->n_tracks_lost             = 2;  // 1 (the first) track is lost each time trackFeatures is called
+        processor_1                       = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+        processor_1->setName("Processor 1");
+        processor_1->link(sensor);
+        processor_2 = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+        processor_2->setName("Processor 2");
+        processor_2->link(sensor);
+
+        problem->print(4, 1, 1, 1);
+    }
+};
+
+// TEST_F(ProcessorTrackerFeatureDummyDoubledTest, installProcessor)
+// {
+//     ASSERT_EQ(processor_1->getProblem(), problem);
+//     ASSERT_EQ(processor_2->getProblem(), problem);
+//     ASSERT_TRUE(problem->check(0));
+// }
+
+// TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_one_prc)
+// {
+//     // Create a capture
+//     CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+//     // Process by one processor
+//     processor_1->captureCallback(capture);
+
+//     problem->print(4,1,1,1);
+// }
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_once_one_prc_then_other)
+{
+    // Create a capture
+    CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+    // Process by one processor
+    processor_1->captureCallback(capture);
+    auto frame_1 = capture->getFrame();
+
+    // Process by the other processor
+    processor_2->captureCallback(capture);
+    auto frame_2 = capture->getFrame();
+
+    problem->print(4, 1, 1, 1);
+
+    ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 1);
+
+    ASSERT_EQ(frame_1, frame_2);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_once)
+{
+    // Process by both processors automatically in sequence
+    CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+    capture->process();
+    problem->print(4, 1, 1, 1);
+
+    ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_twice)
+{
+    // Process by both processors automatically in sequence
+
+    CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+    capture->process();
+
+    capture = std::make_shared<CaptureVoid>(1, sensor);
+    capture->process();
+
+    problem->print(4, 1, 1, 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_thrice)
+{
+    // Create a capture
+    CaptureBasePtr capture = std::make_shared<CaptureVoid>(0, sensor);
+
+    // Process by both processors automatically in sequence
+    capture->process();
+
+    capture = std::make_shared<CaptureVoid>(1, sensor);
+    capture->process();
+
+    capture = std::make_shared<CaptureVoid>(2, sensor);
+    capture->process();
+
+    problem->print(2, 1, 1, 1);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyDoubledTest, process_loop)
+{
+    for (TimeStamp t(0); t <= 6; t = t + 1)
+    {
+        auto capture = std::make_shared<CaptureVoid>(t, sensor);
+        capture->process();
+    }
+    problem->print(2, 1, 1, 1);
+}
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}