From 4311bfca7478acca32c178ec215290535b7b4cd5 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 18 May 2020 10:55:51 +0200
Subject: [PATCH] Temporary fixes & debug info
---
include/core/trajectory/trajectory_base.h | 1 +
src/processor/processor_motion.cpp | 3 ++-
src/processor/processor_tracker.cpp | 5 +++--
src/trajectory/trajectory_base.cpp | 13 ++++++++++---
test/gtest_processor_base.cpp | 2 +-
test/gtest_processor_tracker_feature_dummy.cpp | 1 +
6 files changed, 18 insertions(+), 7 deletions(-)
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 75a336731..9c3729a61 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -96,6 +96,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
+ void printDebug();
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 36425f794..d8b3e6744 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -459,9 +459,10 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
+ auto new_ts = origin_ts + 1e-9;
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
_origin_frame->getState(),
- origin_ts);
+ new_ts);
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index f326eb329..ae9444543 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -107,8 +107,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITHOUT_PACK :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
-
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+auto test_stamp = incoming_ptr_->getTimeStamp()+2e-9;
+WOLF_DEBUG("TIME STAMP ", test_stamp);
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()+2e-9);
incoming_ptr_->link(frm);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index cffa28dcf..1cf3a913e 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -20,8 +20,10 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
-
+ std::cout << "Inserting " << _frame_ptr->getTimeStamp() << " " << _frame_ptr << std::endl;
+ auto success = frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
+ std::cout << "Sucess? " << success.second << std::endl;
+ printDebug();
if (_frame_ptr->isKeyOrAux())
{
// update last_estimated and last_key
@@ -35,7 +37,7 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
// add to list
// frame_list_.erase(_frame_ptr);
- frame_list_.erase(_frame_ptr->id());
+ frame_list_.erase(_frame_ptr->getTimeStamp());
// update last_estimated and last_key
if (_frame_ptr->isKeyOrAux())
@@ -124,4 +126,9 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
+void TrajectoryBase::printDebug()
+{
+ for( auto it : frame_list_ )
+ WOLF_DEBUG("KEY ", it.first, " VALUE ", it.second);
+}
} // namespace wolf
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1665ca2df..25da38b6b 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -115,7 +115,6 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_odo->setTimeStamp(t);
std::cout << "2\n";
- problem->check(1);
// auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
// auto last_ptr = proc_odo_motion->last_ptr_;
// auto last_ptr_frame = last_ptr->getFrame();
@@ -125,6 +124,7 @@ TEST(ProcessorBase, KeyFrameCallback)
// Track
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
std::cout << "4\n";
+ problem->print(4,1,1,1, std::cout);
proc_trk->captureCallback(capt_trk);
std::cout << "5\n";
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 604cd09b4..096dd2554 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -311,6 +311,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
//4TH TIME
WOLF_INFO("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+ problem->getTrajectory()->printDebug();
cap4->process();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
--
GitLab