diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index bf70934d9e820fd910ee7de744b6896b261ae1f4..2c66b5019e3588854cf43cdd0c1006a698f2ce8c 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -20,13 +20,19 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
* 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
*/
+ WOLF_INFO("ProcessorLoopClosure::processCapture capture ", _capture->id());
+
// CASE 1:
if (_capture->getFrame())
{
+ WOLF_INFO("CASE 1");
+
process(_capture->getFrame(), _capture);
// remove the frame and older frames
buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+
+ return;
}
// Search for any stored frame within time tolerance of capture
@@ -35,16 +41,20 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
// CASE 2:
if (frame_pack)
{
+ WOLF_INFO("CASE 2");
+
_capture->link(frame_pack->key_frame);
process(frame_pack->key_frame, _capture);
// remove the frame and older frames
buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+
+ return;
}
// CASE 3:
- else
- buffer_capture_.add(_capture->getTimeStamp(), _capture);
+ WOLF_INFO("CASE 3");
+ buffer_capture_.add(_capture->getTimeStamp(), _capture);
}
void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
@@ -56,14 +66,20 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
* 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
*/
+ WOLF_INFO("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+
// CASE 1:
auto cap = _frame->getCaptureOf(getSensor());
if (cap)
{
+ WOLF_INFO("CASE 1");
+
process(_frame, cap);
// remove the capture (if stored)
buffer_capture_.getContainer().erase(cap->getTimeStamp());
+
+ return;
}
// Search for any stored capture within time tolerance of frame
@@ -72,6 +88,10 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
// CASE 2:
if (capture)
{
+ WOLF_INFO("CASE 2");
+
+ capture->link(_frame);
+
process(_frame, capture);
// remove the capture (if stored)
@@ -79,30 +99,43 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
// remove old captures (10s of old captures are kept in case frames arrives unordered)
buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+
+ return;
}
// CASE 3:
- else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
{
+ WOLF_INFO("CASE 3");
+
// store frame
buffer_pack_kf_.add(_frame, _time_tolerance);
+
+ return;
}
// CASE 4:
+ WOLF_INFO("CASE 4");
// nothing (discard frame)
}
void ProcessorLoopClosure::process(FrameBasePtr _frame, CaptureBasePtr _capture)
{
+ WOLF_INFO("ProcessorLoopClosure::process frame ", _frame->id(), " capture ", _capture->id());
assert(_capture->getFrame() == _frame && "ProcessorLoopClosure::process _capture not linked to _frame");
// Detect and emplace features
+ WOLF_INFO("emplacing features...");
emplaceFeatures(_capture);
// Vote for loop closure search
if (voteFindLoopClosures(_capture))
{
+ WOLF_INFO("finding loop closures...");
+
// Find loop closures
auto cap_lc_list = findLoopClosures(_capture);
+ WOLF_INFO(cap_lc_list.size(), " loop closures found");
+
// Emplace factors for each LC if validated
for (auto cap_lc : cap_lc_list)
if (validateLoopClosure(cap_lc, _capture))
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 2cd9fe39c0142018dcf88fe558e492b38518671c..aec8092de22eaee6a4577b43d8b7a10f8528582b 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -13,60 +13,311 @@
using namespace wolf;
using namespace Eigen;
-// Wolf problem
-ProblemPtr problem = Problem::create("PO", 2);
+class ProcessorLoopClosureTest : public testing::Test
+{
+ protected:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureDummyPtr processor;
+
+ virtual void SetUp()
+ {
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
+ params->time_tolerance = 0.5;
+ processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
+ params);
+ }
-ProcessorLoopClosureDummyPtr proc_lc;
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
-void setup()
+ CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureBase>(frame,
+ "CaptureBase",
+ frame->getTimeStamp(),
+ sensor);
+ }
+
+ CaptureBasePtr createCapture(TimeStamp ts)
+ {
+ // new capture
+ return std::make_shared<CaptureBase>("CaptureBase",
+ ts,
+ sensor);
+ }
+};
+
+TEST_F(ProcessorLoopClosureTest, installProcessor)
{
- // Emplace sensor
- auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorBase",
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
- nullptr,
- 2);
-
- // Emplace processor
- ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
- proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
}
-TEST(ProcessorLoopClosure, installProcessor)
+TEST_F(ProcessorLoopClosureTest, capture_stored)
{
- setup();
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
- EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
- EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
}
-TEST(ProcessorLoopClosure, frame_stored)
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase1)
{
- setup();
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
+{
// new frame
- auto fr1 = problem->emplaceFrame(0, Vector3d::Zero());
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ auto cap1 = createCapture(1);
// keyframecallback
- problem->keyFrameCallback(fr1, nullptr, 1);
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
- EXPECT_EQ(proc_lc->getNStoredFrames(), 1);
- EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
}
-TEST(ProcessorLoopClosure, capture_stored)
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
{
- setup();
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
// new capture
- auto cap1 = std::make_shared<CaptureBase>("CaptureBase", 0, nullptr);
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
// keyframecallback
- proc_lc->captureCallback(cap1);
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ auto frm3 = emplaceFrame(3, Vector3d::Zero());
+ auto frm4 = emplaceFrame(4, Vector3d::Zero());
+ auto frm5 = emplaceFrame(5, Vector3d::Zero());
+ // new captures
+ auto cap4 = createCapture(4);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm3, nullptr, 0.5);
+ problem->keyFrameCallback(frm4, nullptr, 0.5);
+ problem->keyFrameCallback(frm5, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(frm1->getCaptureList().size(), 0);
+ EXPECT_EQ(frm2->getCaptureList().size(), 0);
+ EXPECT_EQ(frm3->getCaptureList().size(), 0);
+ EXPECT_EQ(frm4->getCaptureList().size(), 1);
+ EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+ EXPECT_TRUE(cap4->getFrame() == frm4);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ auto cap1 = createCapture(1);
+ auto cap2 = createCapture(2);
+ auto cap3 = createCapture(3);
+ auto cap4 = createCapture(4);
+ auto cap5 = createCapture(5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_TRUE(cap2->getFrame() == frm2);
+ EXPECT_TRUE(cap3->getFrame() == nullptr);
+ EXPECT_TRUE(cap4->getFrame() == nullptr);
+ EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().size(), 0);
+ EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureTest, emplaceFactors)
+{
+ // emplace frame and capture
+ auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
+ processor->captureCallback(cap1);
+
+ auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
+ processor->captureCallback(cap2);
+
+ auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
+ processor->captureCallback(cap3);
+
+ auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 1);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
- EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
- EXPECT_EQ(proc_lc->getNStoredCaptures(), 1);
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
}
int main(int argc, char **argv)