Resolve "multiple processors per sensor"

Closes #489 (closed)

src/processor/processor_tracker.cpp

@@ -95,25 +95,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             }
             else
             {
-                WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
-                // Append incoming to KF
+                WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed by any other processor!")
+                
+                // Join KF
                 incoming_ptr_->link(keyframe_from_callback);
             }
 
-
-            // // Check if incoming has already a Frame
-            // auto frame_incoming = incoming_ptr_->getFrame();
-            // if (frame_incoming != nullptr and keyframe_from_callback == frame_incoming)
-            // {
-            //     WOLF_DEBUG("PT ", getName(), " Incoming capture had been processed previously!")
-            // }
-            // else
-            // {
-            //     WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
-            //     // Append incoming to 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.
             if (not getProblem()->getMap()->getLandmarkList().empty())
@@ -133,36 +120,19 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
 
-
-            FrameBasePtr keyframe;
-
             // Check if incoming has already a Frame
             auto frame_incoming = incoming_ptr_->getFrame();
-            if (frame_incoming != nullptr)
-            {
-                WOLF_DEBUG("PT ", getName(), "Incoming capture had been processed previously!")
-                assert(frame_incoming->getCaptureOf(getSensor()) == incoming_ptr_ && " Incoming has a frame, but not from a capture of this sensor");
-                keyframe = frame_incoming;
+            assert(frame_incoming == nullptr and " Incoming capture has been processed and linked by another processor, but no keyframe callback was received!");
 
-                // We do not link incoming to keyframe since it is already linked
+            WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
 
-                // We do not issue KF callback since the KF was already there
-            }
-            else
-            {
-                WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
-
-                // make a new KF at incoming
-                keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
-                                                                    incoming_ptr_->getTimeStamp(),
-                                                                    getProblem()->getFrameStructure(),
-                                                                    getProblem()->getState());
-                // Append incoming to KF
-                incoming_ptr_->link(keyframe);
-
-                // Issue KF callback with new KF
-                getProblem()->keyFrameCallback(keyframe, shared_from_this());
-            }
+            // make a new KF at incoming
+            FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+                                                                incoming_ptr_->getTimeStamp(),
+                                                                getProblem()->getFrameStructure(),
+                                                                getProblem()->getState(incoming_ptr_->getTimeStamp()));
+            // Append incoming to KF
+            incoming_ptr_->link(keyframe);
 
             // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
@@ -170,6 +140,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             if (not getProblem()->getMap()->getLandmarkList().empty())
                 processKnown();
 
+            // Issue KF callback with new KF
+            getProblem()->keyFrameCallback(keyframe, shared_from_this());
+
             // Reset this
             resetDerived();
             // Update pointers
@@ -204,13 +177,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);
@@ -239,21 +209,21 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             processKnown();
 
             // chack if the received KF has a capture of this sensor, and if it matches with last_ptr
-            // check if the callback keyframe has a capture of this sensor
-            auto capture_from_callback = keyframe_from_callback->getCaptureOf(this->getSensor());
-
-            if (last_ptr_ == capture_from_callback)
+            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() != nullptr 
-                        and last_ptr_->getFrame() == keyframe_from_callback 
+                assert( last_ptr_->getFrame() == keyframe_from_callback 
                         and "The keyframe has a Capture from this sensor, but this capture is not last!");
-                WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+
+                // 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!")
-                // Append incoming to KF
+                
+                // join KF
                 last_ptr_->link(keyframe_from_callback);
             }
 
@@ -261,7 +231,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             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);
@@ -299,7 +268,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Establish factors
                 establishFactors();
 
-                // Call the new keyframe callback in order to let the other processors to establish their factors
+                // Call the new keyframe callback in order to let the other processors to join
                 getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
 
 
@@ -307,7 +276,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);
 
                 // Reset this
                 resetDerived();
@@ -326,8 +294,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();