diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 93e14f38c41ad1639c09810b4a26631e0ddc61e5..68db6b50e0abd0e2be06711df994cecd3dcc4693 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 39d5558eb7e2925352a78694531aefdea16ee6f1..9b1e749cda92ff0a039816c63f2c753d144e907b 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 7eb1a10b19fe6ffcc169468c6feb69dd33aed69c..9914ff93adc0157976dab7efae9baf9397d5b17e 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 a289c46385155c50c7b8142b3ecec716f356e093..a3ec1b9e9018e041f0ab0aef448894858c265324 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 1c86c0deb339905b6e26bb6d74c3d3ded892b8e8..76572db00a296bb45c84c71a2f1f42c77ef87b64 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 e0b3fe6455f6c88414ec29ed2d5b10796b22f244..75a336731cf3e0599719261d5fb3f850fc67b5b1 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 0d0c6537fedfc21dec63760d877fb56386885adf..579223b5df36039f1de51542f34fb69ff6958bbc 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 675f428cad06b261e47c1f1bf4a4fb29c6bb6edb..0f65ddb4dcd0587fff4cc9cae0d0f6491c10f437 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 a36417929bab661538736d837b016683c9597792..3bb108fe048051b5fef7ec59ab32e4b4a4eeda3c 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 f2d41a420415f8b0f9fb3ac9f6b5e286036c1160..9218f04206ec627b74a2b88d344c25c2d6c2f63e 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 c917f9b7887f6463912101d483f0886d768c81c5..15354ffd1f77f2bc016b3a0e58bfb28774bf49a8 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 49d5e0ee9995144cea25f78d3707eb35743cbb5c..96484e62a3857ec14a8f65b45f179429b9f96fd3 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 51d164760b35ca82fba8c1b2f39dd48530720bd9..cffa28dcfa965f4dc216808fe0beda9f1de44d12 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 5816567557bdd2ca5779615834a3f48a418bbabe..623c7bec91214b55f7d2d8f2034dbba3e01c5ccb 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 457d41fe43f1adb01345065130f416f8ec785185..8f72b5cfd0a5edfd698bd7a23812443127048f54 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 d2c3b622ee5b165c65a40afea6be9bb3a5bef5ef..9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af 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 33026ee4b5f72b7ce06cdf79c3c765963e0cb64c..2c29665d486e65a68fe9bf8c8a37bfefac8aa080 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 bf878e87c1cb5a277225ac20be11fce7f1b12c5d..ce90b16ae7aeafaa9acec35aed709e41bc2c80fd 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 4ac7acf50f3fc066901858db2b34843c58634af3..814ac262a48c196bb291bf10d2af282300cbc57f 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 5d5bb796004cb66a817a5fb546eee22da35e0ba5..760b15d3559ec6d33f2c73cdae3e5c4583c0691c 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 82c65c284c5f06b518943679ac70ae3f2803bc42..e02037d9d110d781ca1dd639bd83b069ccf86b49 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: