diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 4b957d886096e25884481dfc55ac3422a078f7a5..489ef839b2422be5afd045b45a9b3fd499c598b5 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -93,7 +93,6 @@ 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/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 1b1678799234078031392978e2c9c3ec46724f35..d3a5980e81273f7dfcc64a3aac5d8ef7f80fa601 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -21,10 +21,7 @@ TrajectoryBase::~TrajectoryBase() FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) { // add to list - 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(); + frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr)); return _frame_ptr; } @@ -75,9 +72,4 @@ 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_frame_base.cpp b/test/gtest_frame_base.cpp index 3e5fdd7fd3263726383af28d691d9ef784955f52..4845f757c046bd57cea373b4f26a6d4f6e6e5001 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -81,7 +81,6 @@ TEST(FrameBase, LinksToTree) auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01); auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false); -P->print(4,1,1,1); //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic. ASSERT_FALSE(F2->getConstrainedByList().empty()); @@ -95,8 +94,7 @@ P->print(4,1,1,1); ASSERT_FALSE(F1->getCaptureList().empty()); ASSERT_TRUE(F1->getConstrainedByList().empty()); ASSERT_EQ(F1->getHits() , (unsigned int) 0); -P->print(4,1,1,1); -WOLF_INFO("F2 id", F2->id()); + // F2 has no capture and one factor-by ASSERT_TRUE(F2->getCaptureList().empty()); ASSERT_FALSE(F2->getConstrainedByList().empty()); diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index 096dd25542b869f8ee1b1d41969c7a5dc8378a25..604cd09b4f20ed68e1c44273f85a93c8c1a88c1a 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -311,7 +311,6 @@ 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); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index b942c10e713aaec7329be6f1f62a42adebb148c3..84895fd68a2e0f8a3e39fb93892ec716c051b71e 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -44,7 +44,7 @@ struct DummySolverManager : public SolverManager }; /// Set to true if you want debug info -bool debug = true; +bool debug = false; TEST(TrajectoryBase, ClosestKeyFrame) {