From cca0e23a199a485ddd03abe3b2b73807f76cebf9 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Tue, 14 Jul 2020 15:31:30 +0200
Subject: [PATCH] Multiple renamings
- Remove "Key" from frame-related functions' name
- FrameList -> FrameMap
- Rename FrameList Iterators to FrameMap Iterators
---
demos/demo_analytic_autodiff_factor.cpp | 4 +-
demos/demo_wolf_imported_graph.cpp | 4 +-
demos/hello_wolf/processor_range_bearing.cpp | 2 +-
demos/solver/test_iQR_wolf2.cpp | 4 +-
include/core/common/wolf.h | 2 +-
include/core/factor/factor_base.h | 4 +-
include/core/problem/problem.h | 5 +-
include/core/trajectory/trajectory_base.h | 65 +++++++++----------
src/ceres_wrapper/solver_ceres.cpp | 2 +-
src/problem/problem.cpp | 19 ++----
src/sensor/sensor_base.cpp | 10 +--
src/trajectory/trajectory_base.cpp | 22 +++----
test/gtest_factor_diff_drive.cpp | 4 +-
test/gtest_odom_2d.cpp | 12 ++--
test/gtest_problem.cpp | 8 +--
test/gtest_processor_base.cpp | 2 +-
.../gtest_processor_tracker_feature_dummy.cpp | 6 +-
...gtest_processor_tracker_landmark_dummy.cpp | 6 +-
test/gtest_solver_manager.cpp | 2 +-
test/gtest_trajectory.cpp | 30 ++++-----
test/gtest_tree_manager_sliding_window.cpp | 4 +-
21 files changed, 105 insertions(+), 112 deletions(-)
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index d72f6ce2d..0fd8d42cc 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -274,8 +274,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index d3be8bc31..c0afa76e9 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -301,8 +301,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 08e77f76c..7df38e733 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -46,7 +46,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
if (!kf)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
- kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
+ kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
}
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index de9f6f157..82aab6219 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -363,8 +363,8 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
+ for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
{
if (complex_angle)
state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index a06198842..97f9a50ff 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -237,7 +237,7 @@ WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
class TimeStamp;
-typedef std::map<TimeStamp, FrameBasePtr> FrameList;
+typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 89c3397ef..c600f8a24 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -4,8 +4,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
-class FrameListIter;
-class FrameListRevIter;
+class TrajectoryIter;
+class TrajectoryRevIter;
}
//Wolf includes
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 70d33d427..002015f83 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -306,9 +306,8 @@ class Problem : public std::enable_shared_from_this<Problem>
// Frame getters
FrameBasePtr getLastFrame( ) const;
- FrameBasePtr getLastKeyFrame( ) const;
FrameBasePtr getLastKeyOrAuxFrame( ) const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
/** \brief Give the permission to a processor to create a new key Frame
*
@@ -373,7 +372,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
- bool getLastKeyFrameCovariance(Eigen::MatrixXd& _covariance) const;
+ bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 3fa3c1cba..cb89fa517 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -17,10 +17,10 @@ class FrameBase;
namespace wolf {
-class FrameListIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
{
public:
- FrameListIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
+ TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
: std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
{
@@ -32,10 +32,10 @@ class FrameListIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
}
};
-class FrameListRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
+class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
{
public:
- FrameListRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
+ TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
: std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
{
@@ -46,9 +46,9 @@ class FrameListRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse
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;
+// typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_iterator TrajectoryIter;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator TrajectoryRevIter;
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
@@ -56,7 +56,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
friend FrameBase;
private:
- FrameList frame_list_;
+ FrameMap frame_map_;
protected:
std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
@@ -68,15 +68,14 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
~TrajectoryBase() override;
// Frames
- const FrameList& getFrameList() const;
+ const FrameMap& getFrameMap() const;
FrameBasePtr getLastFrame() const;
FrameBasePtr getFirstFrame() const;
- FrameBasePtr getLastKeyFrame() const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
- FrameListIter begin() const;
- FrameListIter end() const;
- FrameListRevIter rbegin() const;
- FrameListRevIter rend() const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ TrajectoryIter begin() const;
+ TrajectoryIter end() const;
+ TrajectoryRevIter rbegin() const;
+ TrajectoryRevIter rend() const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -102,40 +101,40 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void getFactorList(FactorBasePtrList & _fac_list) const;
};
-inline const FrameList& TrajectoryBase::getFrameList() const
+inline const FrameMap& TrajectoryBase::getFrameMap() const
{
- return frame_list_;
- // return frame_list_;
+ return frame_map_;
+ // return frame_map_;
}
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
-{
- auto it = static_cast<FrameListRevIter>(frame_list_.rbegin());
- return *it;
-}
+// inline FrameBasePtr TrajectoryBase::getLastFrame() const
+// {
+// auto it = static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+// return *it;
+// }
inline FrameBasePtr TrajectoryBase::getFirstFrame() const
{
- auto it = static_cast<FrameListIter>(frame_list_.begin());
+ auto it = static_cast<TrajectoryIter>(frame_map_.begin());
return *it;
}
-inline FrameListIter TrajectoryBase::begin() const
+inline TrajectoryIter TrajectoryBase::begin() const
{
- return static_cast<FrameListIter>(frame_list_.begin());
+ return static_cast<TrajectoryIter>(frame_map_.begin());
}
-inline FrameListIter TrajectoryBase::end() const
+inline TrajectoryIter TrajectoryBase::end() const
{
- return static_cast<FrameListIter>(frame_list_.end());
+ return static_cast<TrajectoryIter>(frame_map_.end());
}
-inline FrameListRevIter TrajectoryBase::rbegin() const
+inline TrajectoryRevIter TrajectoryBase::rbegin() const
{
- return static_cast<FrameListRevIter>(frame_list_.rbegin());
+ return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
}
-inline FrameListRevIter TrajectoryBase::rend() const
+inline TrajectoryRevIter TrajectoryBase::rend() const
{
- return static_cast<FrameListRevIter>(frame_list_.rend());
+ return static_cast<TrajectoryRevIter>(frame_map_.rend());
}
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
{
// return last_key_frame_ptr_;
auto last = this->rbegin();
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 4c161bfd7..033b0f910 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -165,7 +165,7 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
{
//robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFrame();
+ auto last_key_frame = wolf_problem_->getLastFrame();
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index e50283b26..b151a6929 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -415,7 +415,7 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
{
- const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyFrame();
+ const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
if (last_kf_or_aux)
ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
@@ -449,7 +449,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyFrame();
+ const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
if (last_kf_or_aux)
state_last = last_kf_or_aux->getState(structure);
@@ -498,7 +498,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+ const auto& last_kf_or_aux = trajectory_ptr_->closestFrameToTimeStamp(_ts);
if (last_kf_or_aux)
state_last = last_kf_or_aux->getState(structure);
@@ -859,9 +859,9 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
return success;
}
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
+bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
- FrameBasePtr frm = getLastKeyFrame();
+ FrameBasePtr frm = getLastFrame();
return getFrameCovariance(frm, cov);
}
@@ -945,14 +945,9 @@ FrameBasePtr Problem::getLastFrame() const
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFrame() const
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
- return trajectory_ptr_->getLastKeyFrame();
-}
-
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
-{
- return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+ return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
void Problem::setPriorOptions(const std::string& _mode,
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 6e5713912..fefc4d332 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -220,9 +220,9 @@ CaptureBasePtr SensorBase::lastCapture(void) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- // auto frame_list = getProblem()->getTrajectory()->getFrameList();
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
auto trajectory = getProblem()->getTrajectory();
- FrameListRevIter frame_rev_it = trajectory->rbegin();
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
while (frame_rev_it != trajectory->rend())
{
capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
@@ -242,7 +242,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) const
if (getProblem())
{
auto trajectory = getProblem()->getTrajectory();
- FrameListRevIter frame_rev_it = trajectory->rbegin();
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->isKey())
@@ -264,11 +264,11 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- // auto frame_list = getProblem()->getTrajectory()->getFrameList();
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
auto trajectory = getProblem()->getTrajectory();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameListRevIter frame_rev_it = trajectory->rbegin();
+ TrajectoryRevIter 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 528b25e03..9f91e0216 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -7,7 +7,7 @@ TrajectoryBase::TrajectoryBase() :
NodeBase("TRAJECTORY", "TrajectoryBase")
{
// WOLF_DEBUG("constructed T");
- frame_list_ = FrameList();
+ frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -18,9 +18,9 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- assert(frame_list_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
+ assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
- frame_list_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
+ frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
return _frame_ptr;
}
@@ -28,8 +28,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->getTimeStamp());
+ // frame_map_.erase(_frame_ptr);
+ frame_map_.erase(_frame_ptr->getTimeStamp());
}
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
@@ -39,11 +39,11 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
}
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
FrameBasePtr closest_kf = nullptr;
- //If frame_list_ is empty then closestKeyFrameToTimeStamp is meaningless
- if(not frame_list_.empty())
+ //If frame_map_ is empty then closestFrameToTimeStamp is meaningless
+ if(not frame_map_.empty())
{
//Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
//In the std they fulfill the following properties:
@@ -51,8 +51,8 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) co
// ub is the first element such that ts < lb.
// The ub definition is fine, and what one would expect. On the other hand the lb definition is NOT the ACTUAL lower bound but the following position
// so, lb = lb_true + 1.
- auto lower_bound = frame_list_.lower_bound(_ts);
- auto upper_bound = frame_list_.upper_bound(_ts);
+ auto lower_bound = frame_map_.lower_bound(_ts);
+ auto upper_bound = frame_map_.upper_bound(_ts);
//We get out of the way the easy cases. If lb is exactly the first element it can mean two things:
// * _ts belongs to the "past", thus the closest frame is the first one
@@ -91,7 +91,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) co
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "") << std::endl;
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 0a914d715..51661f8ab 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -498,7 +498,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
C->process();
}
- auto F2 = problem->getLastKeyFrame();
+ auto F2 = problem->getLastFrame();
// Fix boundaries
F0->fix();
@@ -634,7 +634,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
C->process();
}
- auto F2 = problem->getLastKeyFrame();
+ auto F2 = problem->getLastFrame();
// VectorComposite x2c(x2, "PO", {2,1});
F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index d432e4482..3b525a12e 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -228,7 +228,7 @@ TEST(Odom2d, VoteForKfAndSolve)
solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
- // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
+ // std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
@@ -426,9 +426,9 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceKeyFrame(t_split, x_split);
// there must be 2KF and one F
- ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 2);
+ ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
// The last KF must have TS = 0.08
- ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+ ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -446,9 +446,9 @@ TEST(Odom2d, KF_callback)
// std::cout << report << std::endl;
solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
- ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
@@ -493,7 +493,7 @@ TEST(Odom2d, KF_callback)
// check other KF in the future of the split KF
MatrixXd KF2_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index be4f4b60f..657ddc48a 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -132,7 +132,7 @@ TEST(Problem, SetOrigin_PO_2d)
F->getFactorList(fac_list);
// check that we have one frame (prior)
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
// check that we have one capture (prior)
ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
// check that we have two features (prior: one per state block)
@@ -191,7 +191,7 @@ TEST(Problem, SetOrigin_PO_3d)
F->getFactorList(fac_list);
// check that we have one frame (prior)
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
// check that we have one capture (prior)
ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
// check that we have two features (prior: one per state block)
@@ -229,7 +229,7 @@ TEST(Problem, emplaceFrame_factory)
FrameBasePtr f2 = P->emplaceKeyFrame(2, "POV", 3, VectorXd(10) );
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
ASSERT_EQ(f0->getStateVector().size(), 3 );
ASSERT_EQ(f1->getStateVector().size(), 7 );
@@ -477,7 +477,7 @@ TEST(Problem, check)
// remove stuff from problem, and re-check
- auto F = problem->getLastKeyFrame();
+ auto F = problem->getLastFrame();
auto cby = F->getConstrainedByList().front();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index a47822ae8..c3277078d 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -134,7 +134,7 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "6\n";
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 );
+ ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 );
}
}
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 6238a5786..7b6dc1c40 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -287,8 +287,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
- ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id());
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap1->getFrame()->id());
ASSERT_TRUE(problem->check(0));
//2ND TIME
@@ -322,7 +322,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
cap5->process();
ASSERT_TRUE(cap4->getFrame()->isKey());
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
ASSERT_TRUE(problem->check(0));
// check factors
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 44dfbdf14..d0fe1b1d8 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -321,8 +321,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
- ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame());
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap1->getFrame());
//2ND TIME
WOLF_DEBUG("Second time...");
@@ -351,7 +351,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame());
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap4->getFrame());
ASSERT_EQ(processor->getOrigin(), cap4);
ASSERT_EQ(processor->getLast(), cap5);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 934b32666..30bf42ca3 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -580,7 +580,7 @@ TEST(SolverManager, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameList().size() > 10)
+ if (P->getTrajectory()->getFrameMap().size() > 10)
(*P->getTrajectory()->begin())->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index fd9472999..ff103bc09 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -44,22 +44,22 @@ TEST(TrajectoryBase, ClosestKeyFrame)
FrameBasePtr KF; // closest key-frame queried
- KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2
+ KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+ KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
+ KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(4.2); // after the last frame --> return F2
+ KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
}
@@ -82,14 +82,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// add F1
FrameBasePtr F1 = P->emplaceKeyFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
FrameBasePtr F2 = P->emplaceKeyFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
@@ -98,11 +98,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// P->getDim(),
std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -112,23 +112,23 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// remove frames and keyframes
F2->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
std::cout << __LINE__ << std::endl;
F3->remove(); // F
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
F1->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0);
std::cout << __LINE__ << std::endl;
N.update();
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index a92607e2d..b8ff8240e 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -79,7 +79,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
P->applyPriorOptions(0);
// FRAME 1 ----------------------------------------------------------
- auto F1 = P->getTrajectory()->getLastKeyFrame();
+ auto F1 = P->getTrajectory()->getLastFrame();
ASSERT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
@@ -175,7 +175,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
P->applyPriorOptions(0);
// FRAME 1 (prior) ----------------------------------------------------------
- auto F1 = P->getTrajectory()->getLastKeyFrame();
+ auto F1 = P->getTrajectory()->getLastFrame();
ASSERT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
--
GitLab