From 61bcc41a1a2bfcd80b233a8de28c3b5e82d593ba Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 6 May 2020 12:20:02 +0200
Subject: [PATCH] WIP
---
demos/hello_wolf/hello_wolf.cpp | 4 +-
demos/hello_wolf/hello_wolf_autoconf.cpp | 4 +-
include/core/capture/capture_base.h | 1 +
include/core/common/wolf.h | 10 ++-
include/core/factor/factor_base.h | 4 +-
include/core/trajectory/trajectory_base.h | 69 +++++++++++++++++--
src/ceres_wrapper/ceres_manager.cpp | 4 +-
src/factor/factor_base.cpp | 2 +-
src/frame/frame_base.cpp | 13 ++--
src/problem/problem.cpp | 4 +-
src/processor/processor_motion.cpp | 10 +--
src/sensor/sensor_base.cpp | 20 +++---
src/trajectory/trajectory_base.cpp | 45 +++---------
.../tree_manager_sliding_window.cpp | 2 +-
test/gtest_emplace.cpp | 30 ++++----
test/gtest_factor_base.cpp | 2 +-
test/gtest_odom_2d.cpp | 4 +-
test/gtest_problem.cpp | 2 +-
test/gtest_processor_base.cpp | 1 +
test/gtest_processor_loopclosure.cpp | 2 +-
test/gtest_trajectory.cpp | 2 +-
21 files changed, 131 insertions(+), 104 deletions(-)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 93e14f38c..68db6b50e 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -225,7 +225,7 @@ int main()
for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : *problem->getTrajectory())
if (kf->isKeyOrAux())
for (auto& pair_key_sb : kf->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
@@ -245,7 +245,7 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : *problem->getTrajectory())
if (kf->isKeyOrAux())
{
Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 39d5558eb..9b1e749cd 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -210,7 +210,7 @@ int main()
for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : *problem->getTrajectory())
if (kf->isKeyOrAux())
for (auto& pair_key_sb : kf->getStateBlockMap())
if (pair_key_sb.second && !pair_key_sb.second->isFixed())
@@ -230,7 +230,7 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : *problem->getTrajectory())
if (kf->isKeyOrAux())
{
Eigen::MatrixXd cov;
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 7eb1a10b1..9914ff93a 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -176,6 +176,7 @@ inline unsigned int CaptureBase::id() const
inline FrameBasePtr CaptureBase::getFrame() const
{
+ frame_ptr_.expired();
return frame_ptr_.lock();
}
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index a289c4638..a3ec1b9e9 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -240,16 +240,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
+
// - Frame
WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
-struct FrameComp{
- bool operator() (const FrameBasePtr& lhs, const FrameBasePtr& rhs) const;
-};
-typedef std::set<FrameBasePtr, FrameComp> FrameList;
-typedef std::set<FrameBasePtr, FrameComp>::const_iterator FrameListIter;
-typedef std::set<FrameBasePtr, FrameComp>::const_reverse_iterator FrameListRevIter;
+class TimeStamp;
+typedef std::map<TimeStamp, FrameBasePtr> FrameList;
+
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
WOLF_LIST_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 1c86c0deb..76572db00 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -4,6 +4,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
+class FrameListIter;
+class FrameListRevIter;
}
//Wolf includes
@@ -73,7 +75,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FactorStatus _status = FAC_ACTIVE);
FactorBase(const std::string& _tp,
- const FrameList& _frame_other_list,
+ const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list,
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index e0b3fe645..75a336731 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -6,17 +6,50 @@
namespace wolf{
class Problem;
class FrameBase;
-class TimeStamp;
}
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/common/time_stamp.h"
//std includes
namespace wolf {
+class FrameListIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+{
+ public:
+ FrameListIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
+ }
+};
+
+class FrameListRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
+{
+ public:
+ FrameListRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
+ }
+};
+// typedef std::map<TimeStamp, FrameBasePtr> FrameList;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_iterator FrameListIter;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator FrameListRevIter;
+
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
@@ -40,11 +73,15 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
// Frames
const FrameList& getFrameList() const;
FrameBasePtr getLastFrame() const;
+ FrameBasePtr getFirstFrame() const;
FrameBasePtr getLastKeyFrame() const;
FrameBasePtr getLastKeyOrAuxFrame() const;
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
- void sortFrame(FrameBasePtr _frm_ptr);
+ FrameListIter begin() const;
+ FrameListIter end() const;
+ FrameListRevIter rbegin() const;
+ FrameListRevIter rend() const;
void updateLastFrames();
virtual void printHeader(int depth, //
@@ -66,10 +103,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
public:
// factors
void getFactorList(FactorBasePtrList & _fac_list) const;
-
- protected:
- FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr);
- void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place);
};
inline const FrameList& TrajectoryBase::getFrameList() const
@@ -80,7 +113,29 @@ inline const FrameList& TrajectoryBase::getFrameList() const
inline FrameBasePtr TrajectoryBase::getLastFrame() const
{
- return *(frame_list_.rbegin());
+ auto it = static_cast<FrameListRevIter>(frame_list_.rbegin());
+ return *it;
+}
+inline FrameBasePtr TrajectoryBase::getFirstFrame() const
+{
+ auto it = static_cast<FrameListIter>(frame_list_.begin());
+ return *it;
+}
+inline FrameListIter TrajectoryBase::begin() const
+{
+ return static_cast<FrameListIter>(frame_list_.begin());
+}
+inline FrameListIter TrajectoryBase::end() const
+{
+ return static_cast<FrameListIter>(frame_list_.end());
+}
+inline FrameListRevIter TrajectoryBase::rbegin() const
+{
+ return static_cast<FrameListRevIter>(frame_list_.rbegin());
+}
+inline FrameListRevIter TrajectoryBase::rend() const
+{
+ return static_cast<FrameListRevIter>(frame_list_.rend());
}
inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0d0c6537f..579223b5d 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -88,7 +88,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
+ for(auto fr_ptr : *wolf_problem_->getTrajectory())
if (fr_ptr->isKeyOrAux())
for (const auto& key : fr_ptr->getStructure())
{
@@ -115,7 +115,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
+ for(auto fr_ptr : *wolf_problem_->getTrajectory())
if (fr_ptr->isKeyOrAux())
for (const auto& key1 : wolf_problem_->getFrameStructure())
for (const auto& key2 : wolf_problem_->getFrameStructure())
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 675f428ca..0f65ddb4d 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -36,7 +36,7 @@ FactorBase::FactorBase(const std::string& _tp,
}
FactorBase::FactorBase(const std::string& _tp,
- const FrameList& _frame_other_list,
+ const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list,
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a36417929..3bb108fe0 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -148,8 +148,6 @@ void FrameBase::remove(bool viral_remove_empty_parent)
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
- if (isKeyOrAux() && getTrajectory() != nullptr)
- getTrajectory()->sortFrame(shared_from_this());
}
void FrameBase::setNonEstimated()
@@ -162,7 +160,6 @@ void FrameBase::setNonEstimated()
type_ = NON_ESTIMATED;
if (getTrajectory())
{
- getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
@@ -177,7 +174,6 @@ void FrameBase::setKey()
if (getTrajectory())
{
- getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
@@ -191,7 +187,6 @@ void FrameBase::setAux()
if (getTrajectory())
{
- getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
@@ -206,12 +201,12 @@ FrameBasePtr FrameBase::getPreviousFrame() const
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
- for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
+ for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ )
{
if ( this->frame_id_ == (*f_it)->id() )
{
f_it++;
- if (f_it != getTrajectory()->getFrameList().rend())
+ if (f_it != getTrajectory()->rend())
{
return *f_it;
}
@@ -227,11 +222,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const
FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
- auto f_it = getTrajectory()->getFrameList().rbegin();
+ auto f_it = getTrajectory()->rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory
- while (f_it != getTrajectory()->getFrameList().rend())
+ while (f_it != getTrajectory()->rend())
{
if ( this->frame_id_ == (*f_it)->id())
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f2d41a420..9218f0420 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1161,7 +1161,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// Frames =======================================================================================
- for (auto F : T->getFrameList())
+ for (auto F : *(T))
{
if (_verbose) {
_stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
@@ -1731,7 +1731,7 @@ void Problem::perturb(double amplitude)
S->perturb(amplitude);
// Frames and Captures
- for (auto& F : getTrajectory()->getFrameList())
+ for (auto& F : *(getTrajectory()))
{
if (F->isKeyOrAux())
{
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c917f9b78..15354ffd1 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -301,8 +301,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
+ auto test_last = last_ptr_->getFrame();
+ WOLF_TRACE("HELLO KITTY ", test_last == nullptr);
+ test_last->setTimeStamp(getCurrentTimeStamp());
+ test_last->setState(getProblem()->getState(getCurrentTimeStamp()));
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -600,8 +602,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->rend();
++frame_rev_iter)
{
frame = *frame_rev_iter;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 49d5e0ee9..96484e62a 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -223,9 +223,10 @@ CaptureBasePtr SensorBase::lastCapture(void) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- auto frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameListRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ // auto frame_list = getProblem()->getTrajectory()->getFrameList();
+ auto trajectory = getProblem()->getTrajectory();
+ FrameListRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
@@ -243,9 +244,9 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- auto frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameListRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ auto trajectory = getProblem()->getTrajectory();
+ FrameListRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->isKey())
{
@@ -266,11 +267,12 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- auto frame_list = getProblem()->getTrajectory()->getFrameList();
+ // auto frame_list = getProblem()->getTrajectory()->getFrameList();
+ auto trajectory = getProblem()->getTrajectory();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameListRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ FrameListRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->getTimeStamp() <= _ts)
{
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 51d164760..cffa28dcf 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -17,21 +17,13 @@ TrajectoryBase::~TrajectoryBase()
//
}
-bool FrameComp::operator() (const FrameBasePtr& lhs, const FrameBasePtr& rhs) const
-{
- return lhs->getTimeStamp()<rhs->getTimeStamp();
-}
-
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.insert(_frame_ptr);
+ frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
if (_frame_ptr->isKeyOrAux())
{
- // sort
- sortFrame(_frame_ptr);
-
// update last_estimated and last_key
updateLastFrames();
}
@@ -42,7 +34,8 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.erase(_frame_ptr);
+ // frame_list_.erase(_frame_ptr);
+ frame_list_.erase(_frame_ptr->id());
// update last_estimated and last_key
if (_frame_ptr->isKeyOrAux())
@@ -51,39 +44,17 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
{
- for(auto fr_ptr : getFrameList())
+ for(auto fr_ptr : *this)
fr_ptr->getFactorList(_fac_list);
}
-void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
-{
- moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
-}
-
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place)
-{
- if (*_place != _frm_ptr)
- {
- frame_list_.erase(_frm_ptr->id());
- frame_list_.insert(_place, _frm_ptr);
- updateLastFrames();
- }
-}
-
-FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
-{
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
- return frm_rit.base();
- return getFrameList().begin();
-}
void TrajectoryBase::updateLastFrames()
{
bool last_estimated_set = false;
// NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
+ for (auto frm_rit = rbegin(); frm_rit != rend(); ++frm_rit)
if ((*frm_rit)->isKeyOrAux())
{
if (!last_estimated_set)
@@ -105,7 +76,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) co
FrameBasePtr closest_kf = nullptr;
double min_dt = 1e9;
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
+ for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++)
if ((*frm_rit)->isKey())
{
double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
@@ -126,7 +97,7 @@ FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _t
FrameBasePtr closest_kf = nullptr;
double min_dt = 1e9;
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
+ for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++)
if ((*frm_rit)->isKeyOrAux())
{
double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
@@ -149,7 +120,7 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
- for (auto F : getFrameList())
+ for (auto F : *this)
F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 581656755..623c7bec9 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -7,7 +7,7 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
{
int n_kf(0);
FrameBasePtr first_KF(nullptr), second_KF(nullptr);
- for (auto frm : getProblem()->getTrajectory()->getFrameList())
+ for (auto frm : *getProblem()->getTrajectory())
{
if (frm->isKey())
{
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 457d41fe4..8f72b5cfd 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -43,7 +43,7 @@ TEST(Emplace, Frame)
ASSERT_NE(P->getTrajectory(), nullptr);
FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
TEST(Emplace, Processor)
@@ -68,11 +68,11 @@ TEST(Emplace, Capture)
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
}
@@ -82,16 +82,16 @@ TEST(Emplace, Feature)
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXd::Identity(2,2);
FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
}
TEST(Emplace, Factor)
@@ -100,16 +100,16 @@ TEST(Emplace, Factor)
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
@@ -130,7 +130,7 @@ TEST(Emplace, EmplaceDerived)
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
TEST(Emplace, Nullpointer)
{
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index d2c3b622e..9e4f2ef10 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -53,7 +53,7 @@ class FactorDummy : public FactorBase
{
//
}
- FactorDummy(const FrameList& _frame_other_list,
+ FactorDummy(const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list) :
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 33026ee4b..2c29665d4 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -74,7 +74,7 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
+ for (FrameBasePtr F : *problem->getTrajectory())
{
if (F->isKey())
{
@@ -299,7 +299,7 @@ TEST(Odom2d, VoteForKfAndSolve)
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
- for (auto F : problem->getTrajectory()->getFrameList())
+ for (auto F : *problem->getTrajectory())
{
if (!F->isKey()) break;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index bf878e87c..ce90b16ae 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -411,7 +411,7 @@ TEST(Problem, perturb)
ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
}
- for (auto F : problem->getTrajectory()->getFrameList())
+ for (auto F : *problem->getTrajectory())
{
if (F->isFixed()) // fixed
{
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 4ac7acf50..814ac262a 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -115,6 +115,7 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_odo->setTimeStamp(t);
std::cout << "2\n";
+ problem->check(1);
proc_odo->captureCallback(capt_odo);
std::cout << "3\n";
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 5d5bb7960..760b15d35 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -34,7 +34,7 @@ protected:
virtual bool detectFeatures(CaptureBasePtr cap) { return true;};
virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture)
{
- for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList())
+ for (FrameBasePtr kf : *getProblem()->getTrajectory())
if (kf->isKey())
for (CaptureBasePtr cap : kf->getCaptureList())
if (cap != _capture)
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 82c65c284..e02037d9d 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -263,7 +263,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 0.5 1 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F4->id());
auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 1.5);
// Trajectory status:
--
GitLab