diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index b780457e2d028db1f93ea31999337caf2b51cc0f..e1fea8f6731696817cd3d4c7fad0a4227011c66b 100644
--- a/include/core/landmark/landmark_base.h
+++ b/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();
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index fd1c12350c74419c100b4682e9cdbd9a56370867..8d604258f73f887702cab7d443f7f1072f272c78 100644
--- a/include/core/processor/processor_tracker.h
+++ b/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_;
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 25a62410b1871dbd3d8d69c6481b48a0c0ae0b02..3fe8d24aa87ca7f63cd74fcf3ef0c037c5aeb44e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/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;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 299594805cb46a551145613baf30481f4e7aed22..1709df19170f9110e8e2d180c7d27597a6becc64 100644
--- a/test/CMakeLists.txt
+++ b/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)
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index c7ac5c831f2c03d3588f224946f1ec662a72ab18..f97df272146e529317e64ee2458a973d18c67b4d 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/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)
{
diff --git a/test/gtest_processor_tracker_two_processors_one_sensor.cpp b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2c7b69081cf3a500a2282308ff8e762d237e753
--- /dev/null
+++ b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
@@ -0,0 +1,237 @@
+//--------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();
+}