From c738ed46717d2fb0a47ca5ee1ce4f5d915368f09 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 17 Dec 2020 22:51:57 +0100
Subject: [PATCH] Do not use KEY property of frames
Still a long way to properly delete everything
---
demos/demo_wolf_imported_graph.cpp | 4 +-
demos/hello_wolf/hello_wolf.cpp | 11 +++--
demos/hello_wolf/hello_wolf_autoconf.cpp | 11 +++--
include/core/frame/frame_base.h | 18 +++------
include/core/problem/problem.h | 12 +++---
include/core/trajectory/trajectory_base.h | 9 -----
src/ceres_wrapper/solver_ceres.cpp | 40 +++++++++----------
src/factor/factor_base.cpp | 6 +--
src/frame/frame_base.cpp | 23 ++---------
src/problem/problem.cpp | 9 ++---
src/processor/processor_motion.cpp | 1 -
src/trajectory/trajectory_base.cpp | 1 -
test/gtest_factor_autodiff.cpp | 25 ++++++++----
test/gtest_factor_base.cpp | 4 +-
test/gtest_frame_base.cpp | 22 +++-------
test/gtest_has_state_blocks.cpp | 19 +--------
test/gtest_odom_2d.cpp | 35 +++++++---------
test/gtest_pack_KF_buffer.cpp | 10 ++---
test/gtest_problem.cpp | 17 ++++----
test/gtest_processor_loopclosure.cpp | 7 ++--
.../gtest_processor_tracker_feature_dummy.cpp | 1 -
21 files changed, 110 insertions(+), 175 deletions(-)
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index c0afa76e9..386d80313 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index ef4f17349..2b2d03d43 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -233,12 +233,11 @@ int main()
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf : *problem->getTrajectory())
- if (kf->isKey())
- {
- Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
- }
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index e975bb30f..edeb6b1f0 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -219,12 +219,11 @@ int main()
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf : *problem->getTrajectory())
- if (kf->isKey())
- {
- Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
- }
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 298202a24..a9d1d0e26 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -57,19 +57,16 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
* \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
**/
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+ FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+ FrameBase(const TimeStamp& _ts,
const std::string _frame_structure,
const VectorComposite& _state);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+ FrameBase(const TimeStamp& _ts,
const std::string _frame_structure,
const std::list<VectorXd>& _vectors);
@@ -173,7 +170,7 @@ namespace wolf {
template<typename classType, typename... T>
std::shared_ptr<classType> FrameBase::emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all)
{
- std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::KEY, std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
@@ -181,7 +178,7 @@ std::shared_ptr<classType> FrameBase::emplaceKeyFrame(TrajectoryBasePtr _ptr, T&
template<typename classType, typename... T>
std::shared_ptr<classType> FrameBase::createNonKeyFrame(T&&... all)
{
- std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::REGULAR, std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
return frm;
}
@@ -233,10 +230,7 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
const StateBlockPtr& _sb)
{
- if (isKey())
- HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
- else
- HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
+ HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
return _sb;
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 002015f83..9b86ec2fb 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -206,7 +206,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const double &_time_tol);
/** \brief Emplace frame from string frame_structure, dimension and vector
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
@@ -223,7 +223,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _frame_state State vector; must match the size and format of the chosen frame structure
@@ -240,7 +240,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const VectorComposite& _frame_state);
/** \brief Emplace frame from state
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_state State; must be part of the problem's frame structure
*
@@ -256,7 +256,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
@@ -274,7 +274,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const SizeEigen _dim);
/** \brief Emplace frame from state vector
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_state State vector; must match the size and format of the chosen frame structure
*
@@ -290,7 +290,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
* - The structure is taken from Problem
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index cb89fa517..23939e2e8 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -46,9 +46,6 @@ class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_revers
return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
}
};
-// 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>
@@ -104,14 +101,8 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
inline const FrameMap& TrajectoryBase::getFrameMap() const
{
return frame_map_;
- // return frame_map_;
}
-// inline FrameBasePtr TrajectoryBase::getLastFrame() const
-// {
-// auto it = static_cast<TrajectoryRevIter>(frame_map_.rbegin());
-// return *it;
-// }
inline FrameBasePtr TrajectoryBase::getFirstFrame() const
{
auto it = static_cast<TrajectoryIter>(frame_map_.begin());
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index e0995c7be..56be64659 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -99,12 +99,11 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
for(auto fr_ptr : *wolf_problem_->getTrajectory())
- if (fr_ptr->isKey())
- for (const auto& key : fr_ptr->getStructure())
- {
- const auto& sb = fr_ptr->getStateBlock(key);
- all_state_blocks.push_back(sb);
- }
+ for (const auto& key : fr_ptr->getStructure())
+ {
+ const auto& sb = fr_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -131,23 +130,22 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
{
// first create a vector containing all state blocks
for(auto fr_ptr : *wolf_problem_->getTrajectory())
- if (fr_ptr->isKey())
- for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for (const auto& key1 : wolf_problem_->getFrameStructure())
+ {
+ const auto& sb1 = fr_ptr->getStateBlock(key1);
+ assert(isStateBlockRegisteredDerived(sb1));
+
+ for (const auto& key2 : wolf_problem_->getFrameStructure())
{
- const auto& sb1 = fr_ptr->getStateBlock(key1);
- assert(isStateBlockRegisteredDerived(sb1));
-
- for (const auto& key2 : wolf_problem_->getFrameStructure())
- {
- const auto& sb2 = fr_ptr->getStateBlock(key2);
- assert(isStateBlockRegisteredDerived(sb2));
-
- state_block_pairs.emplace_back(sb1, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
- if (sb1 == sb2)
- break;
- }
+ const auto& sb2 = fr_ptr->getStateBlock(key2);
+ assert(isStateBlockRegisteredDerived(sb2));
+
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
+ break;
}
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index a0b4bc900..c03fe00ad 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -260,10 +260,6 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
- // if frame, should be key
- if (getCapture() and getFrame())
- assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
-
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
this->setProblem(_ftr_ptr->getProblem());
@@ -274,7 +270,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
auto frame_other = frm_ow.lock();
if(frame_other != nullptr)
{
- assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+ assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
frame_other->addConstrainedBy(shared_from_this());
}
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index dedc16670..16a40a916 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -12,15 +12,13 @@ namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
const std::string _frame_structure,
const VectorComposite& _state) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
for (const auto& pair_key_vec : _state)
@@ -35,15 +33,13 @@ FrameBase::FrameBase(const FrameType & _tp,
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
const std::string _frame_structure,
const std::list<VectorXd>& _vectors) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
@@ -62,8 +58,7 @@ FrameBase::FrameBase(const FrameType & _tp,
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _v_ptr) :
@@ -71,7 +66,6 @@ FrameBase::FrameBase(const FrameType & _tp,
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
if (_p_ptr)
@@ -116,8 +110,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
}
// Remove Frame State Blocks
- if ( isKey() )
- removeStateBlocks(getProblem());
+ removeStateBlocks(getProblem());
// remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
@@ -287,7 +280,6 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
assert(this->getTrajectory() == nullptr && "linking an already linked frame");
- assert(isKey() && "Trying to link a non keyframe");
if(_trj_ptr)
{
@@ -303,8 +295,6 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
void FrameBase::setProblem(ProblemPtr _problem)
{
- assert(isKey() && "Trying to setProblem to a non keyframe");
-
if (_problem == nullptr || _problem == this->getProblem())
return;
@@ -410,11 +400,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
<< " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
- // // check trajectory pointer
- // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory()
- // << " but trajectory pointer is" << trajectory_ptr << "\n";
- // log.assertTrue((getTrajectory() == T, inconsistency_explanation);
-
// check constrained_by
for (auto cby : getConstrainedByList())
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index bfcb33fa6..f97b97d73 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1174,12 +1174,9 @@ void Problem::perturb(double amplitude)
// Frames and Captures
for (auto& F : *(getTrajectory()))
{
- if (F->isKey())
- {
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
- C->perturb(amplitude);
- }
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
}
// Landmarks
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index bf44bb4a9..4d06c1d75 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -676,7 +676,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
assert(_origin_frame->getTrajectory() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
- assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
TimeStamp origin_ts = _origin_frame->getTimeStamp();
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 23f967716..4a64613a3 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -6,7 +6,6 @@ namespace wolf {
TrajectoryBase::TrajectoryBase() :
NodeBase("TRAJECTORY", "TrajectoryBase")
{
-// WOLF_DEBUG("constructed T");
frame_map_ = FrameMap();
}
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 0acd77891..9f67d98ab 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -350,8 +350,11 @@ TEST(FactorAutodiff, AutodiffDummy12)
TEST(FactorAutodiff, EmplaceOdom2d)
{
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ FrameBasePtr fr1_ptr = problem->emplaceKeyFrame(TimeStamp(1));
+ FrameBasePtr fr2_ptr = problem->emplaceKeyFrame(TimeStamp(2));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -377,12 +380,14 @@ TEST(FactorAutodiff, EmplaceOdom2d)
TEST(FactorAutodiff, ResidualOdom2d)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceKeyFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceKeyFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -421,12 +426,14 @@ TEST(FactorAutodiff, ResidualOdom2d)
TEST(FactorAutodiff, JacobianOdom2d)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceKeyFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceKeyFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -499,12 +506,14 @@ TEST(FactorAutodiff, JacobianOdom2d)
TEST(FactorAutodiff, AutodiffVsAnalytic)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceKeyFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceKeyFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index e1e035bf0..79663676a 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test
void SetUp() override
{
- F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr);
- F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr);
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 504ea3afe..956567ee8 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -22,7 +22,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(REGULAR, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -31,13 +31,11 @@ TEST(FrameBase, GettersAndSetters)
F->getTimeStamp(t);
ASSERT_EQ(t, 1);
ASSERT_FALSE(F->isFixed());
- ASSERT_EQ(F->isKey(), false);
- ASSERT_EQ(F->isKey(), false);
}
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(REGULAR, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -47,12 +45,10 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(REGULAR, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
- // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
- // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
@@ -115,8 +111,6 @@ TEST(FrameBase, LinksToTree)
ASSERT_FALSE(F1->isFixed());
F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
-
- ASSERT_TRUE(F1->isKey());
}
#include "core/state_block/state_quaternion.h"
@@ -128,7 +122,7 @@ TEST(FrameBase, GetSetState)
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
- FrameBase F(REGULAR, 1, sbp, sbq, sbv);
+ FrameBase F(1, sbp, sbq, sbv);
// Give values to vectors and vector blocks
VectorXd x(10), x1(10), x2(10);
@@ -152,12 +146,10 @@ TEST(FrameBase, GetSetState)
TEST(FrameBase, Constructor_composite)
{
- FrameBase F = FrameBase(KEY,
- 0.0,
+ FrameBase F = FrameBase(0.0,
"POV",
VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
- ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
@@ -165,12 +157,10 @@ TEST(FrameBase, Constructor_composite)
TEST(FrameBase, Constructor_vectors)
{
- FrameBase F = FrameBase(KEY,
- 0.0,
+ FrameBase F = FrameBase(0.0,
"POV",
{Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
- ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index c19d99aa6..b1baad456 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test
sbo1 = std::make_shared<StateQuaternion>();
sbv1 = std::make_shared<StateBlock>(3); // size 3
- F0 = std::make_shared<FrameBase>(REGULAR, 0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(REGULAR, 1.0, nullptr); // non KF
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
}
void TearDown() override{}
@@ -50,21 +50,6 @@ class HasStateBlocksTest : public testing::Test
};
-TEST_F(HasStateBlocksTest, Notifications_setKey_add)
-{
- Notification n;
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
- ASSERT_DEATH(F0->link(problem->getTrajectory()), "");
-
- // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
- F0->setKey(problem);
-
- ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
- ASSERT_EQ(n, ADD);
-}
-
TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
{
Notification n;
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 3b525a12e..c4b9d0979 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -76,27 +76,24 @@ void show(const ProblemPtr& problem)
for (FrameBasePtr F : *problem->getTrajectory())
{
- if (F->isKey())
+ cout << "----- Key Frame " << F->id() << " -----" << endl;
+ if (!F->getCaptureList().empty())
{
- cout << "----- Key Frame " << F->id() << " -----" << endl;
- if (!F->getCaptureList().empty())
+ auto C = F->getCaptureList().front();
+ if (!C->getFeatureList().empty())
{
- auto C = F->getCaptureList().front();
- if (!C->getFeatureList().empty())
- {
- auto f = C->getFeatureList().front();
- cout << " feature measure: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
- << endl;
- cout << " feature covariance: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
- }
+ auto f = C->getFeatureList().front();
+ cout << " feature measure: \n"
+ << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
+ << endl;
+ cout << " feature covariance: \n"
+ << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
}
- cout << " state: \n" << F->getStateVector().transpose() << endl;
- Eigen::MatrixXd cov;
- problem->getFrameCovariance(F,cov);
- cout << " covariance: \n" << cov << endl;
}
+ cout << " state: \n" << F->getStateVector().transpose() << endl;
+ Eigen::MatrixXd cov;
+ problem->getFrameCovariance(F,cov);
+ cout << " covariance: \n" << cov << endl;
}
}
@@ -125,7 +122,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
ProblemPtr Pr = Problem::create("PO", 2);
- SolverCeres solver(Pr);
+ SolverCeres solver(Pr);
// KF0 and absolute prior
FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
@@ -308,8 +305,6 @@ TEST(Odom2d, VoteForKfAndSolve)
int n = 0;
for (auto F : *problem->getTrajectory())
{
- if (!F->isKey()) break;
-
ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 351edf4c9..d1fce41e5 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -34,10 +34,10 @@ class BufferPackKeyFrameTest : public testing::Test
void SetUp(void) override
{
- f10 = std::make_shared<FrameBase>(REGULAR, TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(REGULAR, TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(REGULAR, TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(REGULAR, TimeStamp(28),nullptr,nullptr,nullptr);
+ f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
tt10 = 1.0;
tt20 = 1.0;
@@ -230,7 +230,7 @@ TEST_F(BufferPackKeyFrameTest, removeUpTo)
// Specifically, only f28 should remain
pack_kf_buffer.add(f28, tt28);
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(REGULAR, TimeStamp(22),nullptr,nullptr,nullptr);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index dc44348f5..a0b8971db 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -385,21 +385,22 @@ TEST(Problem, perturb)
//// CHECK ALL STATE BLOCKS ///
- double prec = 1e-2;
+#define prec 1e-2
for (auto S : problem->getHardware()->getSensorList())
{
- ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) );
- ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec );
ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
}
- for (auto F : *problem->getTrajectory())
+ for (auto pair_T_F : problem->getTrajectory()->getFrameMap())
{
+ auto F = pair_T_F.second;
if (F->isFixed()) // fixed
{
- ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) );
- ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec );
}
else
{
@@ -416,8 +417,8 @@ TEST(Problem, perturb)
{
if ( L->isFixed() ) // fixed
{
- ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) );
- ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) );
+ ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec );
+ ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec );
}
else
{
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 6a4b40d32..7f99374f5 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -35,10 +35,9 @@ protected:
CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
{
for (FrameBasePtr kf : *getProblem()->getTrajectory())
- if (kf->isKey())
- for (CaptureBasePtr cap : kf->getCaptureList())
- if (cap != _capture)
- return cap;
+ for (CaptureBasePtr cap : kf->getCaptureList())
+ if (cap != _capture)
+ return cap;
return nullptr;
};
void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 7b6dc1c40..49593d64a 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -321,7 +321,6 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
- ASSERT_TRUE(cap4->getFrame()->isKey());
ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
ASSERT_TRUE(problem->check(0));
--
GitLab