diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 32b582b04f15da321b6dd7710fda60ac9e882cac..14a8f14b799ab39abbccf2e11ded6da84bd2bfda 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -71,6 +71,9 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
+ //Auxiliary constructor, needed to put frametype in front
+ FrameBase(const FrameType & _tp, const std::string _frame_structure, const SizeEigen _dim, const TimeStamp& _ts, const Eigen::VectorXd& _x);
+
virtual ~FrameBase();
virtual void remove(bool viral_remove_empty_parent=false);
@@ -85,7 +88,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// set type
void setNonEstimated();
- void setKey();
+ void setKey(ProblemPtr _prb);
void setAux();
// Frame values ------------------------------------------------
@@ -129,7 +132,9 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
bool isConstrainedBy(const FactorBasePtr& _factor) const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ static std::shared_ptr<classType> emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all);
+ template<typename classType, typename... T>
+ static std::shared_ptr<classType> createNonKeyFrame(T&&... all);
virtual void printHeader(int depth, //
bool constr_by, //
@@ -166,13 +171,20 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
namespace wolf {
template<typename classType, typename... T>
-std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
+std::shared_ptr<classType> FrameBase::emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all)
{
- std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(KEY, std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
+template<typename classType, typename... T>
+std::shared_ptr<classType> FrameBase::createNonKeyFrame(T&&... all)
+{
+ std::shared_ptr<classType> frm = std::make_shared<classType>(NON_ESTIMATED, std::forward<T>(all)...);
+ return frm;
+}
+
inline unsigned int FrameBase::id() const
{
return frame_id_;
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 36e5e338bde88d49062931e383929baa005bc9df..ef5bba9a7ad087b08eae31699556af520b71cd32 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -203,9 +203,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+ FrameBasePtr emplaceKeyFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp);
@@ -220,9 +219,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+ FrameBasePtr emplaceKeyFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without structure
@@ -235,9 +233,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceKeyFrame(const Eigen::VectorXd& _frame_state, //
+ const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without structure nor state
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -248,8 +245,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp);
// Frame getters
FrameBasePtr getLastFrame( ) const;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 3bb108fe048051b5fef7ec59ab32e4b4a4eeda3c..e39265816f31260a4a3c85dee679447f66788cb6 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -111,6 +111,11 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
}
+FrameBase::FrameBase(const FrameType & _tp, const std::string _frame_structure, const SizeEigen _dim, const TimeStamp& _ts, const Eigen::VectorXd& _x):
+ FrameBase(_frame_structure, _dim, _tp, _ts, _x)
+{
+}
+
FrameBase::~FrameBase()
{
}
@@ -164,13 +169,11 @@ void FrameBase::setNonEstimated()
}
}
-void FrameBase::setKey()
+void FrameBase::setKey(ProblemPtr _prb)
{
- // register if previously not estimated
- if (!isKeyOrAux())
- registerNewStateBlocks(getProblem());
-
+ assert(_prb != nullptr && "setting Key fram with a null problem pointer");
type_ = KEY;
+ this->link(_prb->getTrajectory());
if (getTrajectory())
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 9218f04206ec627b74a2b88d344c25c2d6c2f63e..d0cbfaef5b142e5b122413981fd02f4caabd9b0d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -321,35 +321,31 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
+FrameBasePtr Problem::emplaceKeyFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp)
{
- auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _time_stamp, _frame_state);
return frm;
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
+FrameBasePtr Problem::emplaceKeyFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
const TimeStamp& _time_stamp)
{
- return emplaceFrame(_frame_structure, _dim, _frame_key_type, getState(_time_stamp), _time_stamp);
+ return emplaceKeyFrame(_frame_structure, _dim, getState(_time_stamp), _time_stamp);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceKeyFrame(const Eigen::VectorXd& _frame_state, //
+ const TimeStamp& _time_stamp)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
+ return emplaceKeyFrame(this->getFrameStructure(), this->getDim(), _frame_state, _time_stamp);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
+ return emplaceKeyFrame(this->getFrameStructure(), this->getDim(), _time_stamp);
}
Eigen::VectorXd Problem::getCurrentState() const
@@ -945,9 +941,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceFrame(this->getFrameStructure(),
+ prior_keyframe = emplaceKeyFrame(this->getFrameStructure(),
this->getDim(),
- KEY,
prior_options_->state,
_ts);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d8b3e67446fb83a815c6330c587a0634b8e35b41..233e3c0f1f3130511d7ebcf0debd70107ad45bc6 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -301,10 +301,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
- auto test_last = last_ptr_->getFrame();
- WOLF_TRACE("Is last_ptr_ null? ", test_last == nullptr);
- test_last->setTimeStamp(getCurrentTimeStamp());
- test_last->setState(getProblem()->getState(getCurrentTimeStamp()));
+ last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
+ last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -342,7 +340,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Set the frame of last_ptr as key
auto key_frame = last_ptr_->getFrame();
- key_frame ->setKey();
+ key_frame ->setKey(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -351,9 +349,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getProblem()->getCurrentState(),
- getCurrentTimeStamp());
+ auto prb_ptr = getProblem();
+ auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ getCurrentTimeStamp(),
+ prb_ptr->getCurrentState());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -431,7 +431,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
+ FrameBasePtr key_frame_ptr = getProblem()->emplaceKeyFrame(_x_origin, _ts_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -459,10 +459,11 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto new_ts = origin_ts + 1e-9;
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- new_ts);
+ auto prb_ptr = getProblem();
+ auto new_frame_ptr = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ origin_ts,
+ _origin_frame->getState());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index ae9444543bb82649d5a549b86dfa9aeeb2456445..947ca38052ee625d2fbb8452c68b2b2dc565619c 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -78,7 +78,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
+ FrameBasePtr kfrm = getProblem()->emplaceKeyFrame(incoming_ptr_->getTimeStamp());
incoming_ptr_->link(kfrm);
// Process info
@@ -107,9 +107,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITHOUT_PACK :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
-auto test_stamp = incoming_ptr_->getTimeStamp()+2e-9;
-WOLF_DEBUG("TIME STAMP ", test_stamp);
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()+2e-9);
+ auto prb_ptr = getProblem();
+ auto incoming_ts = incoming_ptr_->getTimeStamp();
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ incoming_ts,
+ prb_ptr->getState(incoming_ts));
incoming_ptr_->link(frm);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
@@ -144,7 +147,12 @@ WOLF_DEBUG("TIME STAMP ", test_stamp);
last_old_frame->remove();
// Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ auto prb_ptr = getProblem();
+ auto incoming_ts = incoming_ptr_->getTimeStamp();
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ incoming_ts,
+ prb_ptr->getState(incoming_ts));
incoming_ptr_->link(frm);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -177,10 +185,10 @@ WOLF_DEBUG("TIME STAMP ", test_stamp);
// We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setKey();
+ last_ptr_->getFrame()->setKey(getProblem());
// // make F; append incoming to new F
- // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
// incoming_ptr_->link(frm);
// Establish factors
@@ -193,7 +201,11 @@ WOLF_DEBUG("TIME STAMP ", test_stamp);
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+ auto prb_ptr = getProblem();
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
@@ -211,7 +223,7 @@ WOLF_DEBUG("TIME STAMP ", test_stamp);
last_ptr_->getFrame()->setAuxiliary();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
// Establish factors
@@ -240,7 +252,11 @@ WOLF_DEBUG("TIME STAMP ", test_stamp);
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+ auto prb_ptr = getProblem();
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
+ prb_ptr->getDim(),
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 7ead2b8f801602e664cae1e2ec4249f21ef916a5..22c7990a5fa638986d82118622275a9363c3af6c 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -547,7 +547,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
@@ -589,7 +589,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 8f72b5cfd0a5edfd698bd7a23812443127048f54..c4fa60d9eec6860661fc50c4c8b080cf9c068500 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -42,7 +42,7 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
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));
+ FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
@@ -67,7 +67,7 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
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));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -81,7 +81,7 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
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));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -99,7 +99,7 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
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));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -119,7 +119,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -141,7 +141,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
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));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 14dea0d458afbb8fa6223f0bfc8d286bc2a473e6..1e9f1019dba9e192e522b204519964e3790de9fb 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(problem_ptr->zeroState(), TimeStamp(0));
// Capture, feature and factor
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 985e25a23de5f1c6b137d1b477bb9a22b219fb2a..d089b4dbddfe4e6b84334c6ddcc2a3b0fc47ff09 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -55,9 +55,9 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
ceres_manager = std::make_shared<CeresManager>(problem);
- F1 = problem->emplaceFrame (KEY, pose1, 1.0);
+ F1 = problem->emplaceKeyFrame (pose1, 1.0);
- F2 = problem->emplaceFrame (KEY, pose2, 2.0);
+ F2 = problem->emplaceKeyFrame (pose2, 2.0);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 594eb169754b47fb3304536742ea445051e4da85..bc7b92005a0aa81262599264986ae3497dec7241 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -56,7 +56,7 @@ class FixtureFactorBlockDifference : public testing::Test
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
//FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
- KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
+ KF1_ = problem_->emplaceKeyFrame(problem_->zeroState(), t1);
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index fd82dd83817fc2fe6991f0e8ba6bea09bf8c431e..3fb12bb38ca2f3ea4157842aa4c9e6a6348bccf5 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -162,18 +162,16 @@ class FactorDiffDriveTest : public testing::Test
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
- F0 = FrameBase::emplace<FrameBase>(trajectory,
- "PO",
- 2,
- KEY,
- 0.0,
- Vector3d(0,0,0));
- F1 = FrameBase::emplace<FrameBase>(trajectory,
- "PO",
- 2,
- KEY,
- 1.0,
- Vector3d(1,0,0));
+ F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
+ "PO",
+ 2,
+ 0.0,
+ Vector3d(0,0,0));
+ F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
+ "PO",
+ 2,
+ 1.0,
+ Vector3d(1,0,0));
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 76d559a767f962ef7e82c41bffdded012dd4d04e..954dcc47b185a30f3c1bdaec54c02069a664a505 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index 1315118016ebe3319a04e07b37d0d8438b714ebe..b24a074a3262884d6364df47c6253e98bd2c41a8 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, 7, 6, nullptr);
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 5d9cb7f406beef3b95ce1da4add8f2041a20c685..a302bf7721c587762c8566180fa5168ef3bb9095 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO", 2);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceKeyFrame(problem->zeroState(), TimeStamp(0));
// Capture, feature and factor from frm1 to frm0
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, 3, 3, nullptr);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 9eeb5dfc14bbc6b185c598a8a17489a44794206e..c9e95cca169bb9db68a82b8742d280dc31b22b40 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO", 3);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceKeyFrame(problem->zeroState(), TimeStamp(0));
// Capture, feature and factor
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, 7, 6, nullptr);
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index fd8e2296c887aeb7ce6a07579eec939964d29a92..2a9575ac9e19c72f49fcc9284a47eb9057fe19cd 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 32cff2bd79a00fdcf18850c5f871493584e27865..756e1f9ecc4244756cd60d354270e73ec5196f78 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -27,8 +27,8 @@ auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d
auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 90ba45c62add57f2f5d222579810d39f69b0cf6b..b8dceb6baa616fb4bced7733ed17da639e9c5082 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -69,8 +69,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
@@ -112,7 +112,7 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE(F1->isFixed());
// set key
- F1->setKey();
+ F1->setKey(P);
ASSERT_TRUE(F1->isKey());
ASSERT_TRUE(F1->isKeyOrAux());
}
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index df90859ba57d46382d97e7fa29fa66b23b137580..c32acbd19e304d6b97212f7277616a0b88f426c0 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -59,7 +59,7 @@ TEST_F(HasStateBlocksTest, Notifications_setKey_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
ASSERT_EQ(n, ADD);
@@ -83,7 +83,7 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
ASSERT_EQ(n, ADD);
@@ -100,7 +100,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
// first make KF, then add SB
F1->link(problem->getTrajectory());
- F1->setKey();
+ F1->setKey(problem);
F1->addStateBlock("P", sbp1);
@@ -132,7 +132,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
F0->link(problem->getTrajectory());
F0->addStateBlock("V", sbv0);
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
ASSERT_EQ(n, ADD);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 2c29665d486e65a68fe9bf8c8a37bfefac8aa080..b17170025e860223eec02e3a8cefcc10133f37df 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -132,14 +132,14 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
+ FrameBasePtr F1 = Pr->emplaceKeyFrame(Vector3d::Zero(), t);
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
+ FrameBasePtr F2 = Pr->emplaceKeyFrame(Vector3d::Zero(), t);
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
@@ -416,7 +416,7 @@ TEST(Odom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
Vector3d x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
+ FrameBasePtr keyframe_2 = problem->emplaceKeyFrame(x_split, t_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
@@ -453,7 +453,7 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, x_split, t_split);
+ FrameBasePtr keyframe_1 = problem->emplaceKeyFrame(x_split, t_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index ce90b16ae7aeafaa9acec35aed709e41bc2c80fd..3e7bedc16bf84b9eaa8ca07f8b65723748ac6572 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -239,9 +239,9 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY, VectorXd(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXd(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceKeyFrame("PO", 2, VectorXd(3), TimeStamp(0.0));
+ FrameBasePtr f1 = P->emplaceKeyFrame("PO", 3, VectorXd(7), TimeStamp(1.0));
+ FrameBasePtr f2 = P->emplaceKeyFrame("POV", 3, VectorXd(10), TimeStamp(2.0));
// std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
@@ -280,7 +280,7 @@ TEST(Problem, StateBlocks)
auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
- auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
+ auto KF = P->emplaceKeyFrame("PO", 3, xs3d, 0);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
@@ -337,7 +337,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
+ FrameBasePtr F = P->emplaceKeyFrame("PO", 3, xs3d, 0);
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -373,7 +373,7 @@ TEST(Problem, perturb)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, pose, t);
+ auto F = problem->emplaceKeyFrame(pose, t);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -463,7 +463,7 @@ TEST(Problem, check)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, pose, t);
+ auto F = problem->emplaceKeyFrame(pose, t);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 760b15d3559ec6d33f2c73cdae3e5c4583c0691c..1e8246ee2de40852e7ce3a3df31fec55c32d8619 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -84,7 +84,7 @@ TEST(ProcessorLoopClosure, installProcessor)
// new KF
t += dt;
- auto kf = problem->emplaceFrame(KEY, x, t); //KF2
+ auto kf = problem->emplaceKeyFrame(x, t); //KF2
// emplace a capture in KF
auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
proc_lc->captureCallback(capt_lc);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 8f4738dae79311399a88bf15487c729741eba888..fa989be48e578e94046d10702197db0447f24094 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -248,7 +248,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -293,7 +293,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -338,7 +338,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 640dc71faefcdfcaa937bdc928c904ef618f05d9..fd6e25a8036d4d4c87cd0c39acbdfee9db8b5201 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -515,7 +515,7 @@ TEST(SolverManager, AddFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
@@ -542,7 +542,7 @@ TEST(SolverManager, RemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
@@ -575,7 +575,7 @@ TEST(SolverManager, AddRemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
@@ -610,7 +610,7 @@ TEST(SolverManager, DoubleRemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 1f641dcf0da9765ddd155b5d35eaf87041879828..66a711db5fb62ad804f8df12211653aa154c6357 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -35,11 +35,11 @@ class TrackMatrixTest : public testing::Test
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = FrameBase::emplace<FrameBase>(nullptr, 0.0, nullptr);
- F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, nullptr);
- F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, nullptr);
- F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, nullptr);
- F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, nullptr);
+ F0 = FrameBase::emplaceKeyFrame<FrameBase>(nullptr, 0.0, nullptr);
+ F1 = FrameBase::emplaceKeyFrame<FrameBase>(nullptr, 1.0, nullptr);
+ F2 = FrameBase::emplaceKeyFrame<FrameBase>(nullptr, 2.0, nullptr);
+ F3 = FrameBase::emplaceKeyFrame<FrameBase>(nullptr, 3.0, nullptr);
+ F4 = FrameBase::emplaceKeyFrame<FrameBase>(nullptr, 4.0, nullptr);
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
@@ -50,8 +50,8 @@ class TrackMatrixTest : public testing::Test
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
- F0->setKey();
- F4->setKey();
+ F0->setKey(nullptr);
+ F4->setKey(nullptr);
// link captures
C0->link(F0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index e02037d9d110d781ca1dd639bd83b069ccf86b49..e9951c270aab76a84ed56ff08ebfbf977da8afc7 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -57,10 +57,11 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 3);
- FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 4);
+ FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
+ FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2);
+ //WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
+ FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 4, Eigen::Vector3d::Zero());
FrameBasePtr KF; // closest key-frame queried
@@ -94,9 +95,10 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
+ FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
+ //WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
+ FrameBasePtr F2 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 2, Eigen::Vector3d::Zero());
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
FrameBasePtr KF; // closest key-frame queried
@@ -133,21 +135,21 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1); // 2 non-fixed
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
@@ -203,31 +205,31 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// --+-----+-----+---> time
// add frames and keyframes in random order --> keyframes must be sorted after that
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
+ FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3); // non-key-frame
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero()); // non-key-frame
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
+ FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- F3->setKey(); // set KF3
+ F3->setKey(P); // set KF3
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- auto F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 1.5);
+ auto F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero());
// Trajectory status:
// KF1 KF2 KF3 F4 frames
// 1 2 3 1.5 time stamps
@@ -237,7 +239,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- F4->setKey();
+ F4->setKey(P);
// Trajectory status:
// KF1 KF4 KF2 KF3 frames
// 1 1.5 2 3 time stamps
@@ -265,7 +267,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getLastFrame()->id(), F4->id());
- auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 1.5);
+ //WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
+ auto F5 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero());
// Trajectory status:
// KF4 KF2 AuxF5 KF3 KF2 frames
// 0.5 1 1.5 3 4 time stamps
@@ -285,7 +288,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F6 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 6);
+ auto F6 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 6, Eigen::Vector3d::Zero());
// Trajectory status:
// KF4 KF2 KF3 KF2 AuxF5 F6 frames
// 0.5 1 3 4 5 6 time stamps
@@ -295,7 +298,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F7 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 5.5);
+ auto F7 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 5.5, Eigen::Vector3d::Zero());
// Trajectory status:
// KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames
// 0.5 1 3 4 5 5.5 6 time stamps
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 4f609da767afb85bbc6cc818906524612aff3ded..08cd9f8cfb240fe58bf94a8c52fb19aad365635f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
- auto F0 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(0.0));
+ auto F0 = P->emplaceKeyFrame("PO", 3, VectorXd(7), TimeStamp(0.0));
P->keyFrameCallback(F0, nullptr, 0);
ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 4cf8cf524ef114f357919661a3aee62a65e34234..f171dbcff79a31e07f71b1045f44ec71f9e62a8f 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -87,7 +87,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
+ auto F2 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(2));
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -103,7 +103,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(3));
+ auto F3 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(3));
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -119,7 +119,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(4));
+ auto F4 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(4));
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -139,7 +139,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_TRUE(F2->isFixed()); //Fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(5));
+ auto F5 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(5));
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -183,7 +183,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
+ auto F2 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(2));
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -199,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(3));
+ auto F3 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(3));
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -215,7 +215,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(4));
+ auto F4 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(4));
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -235,7 +235,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F2->isFixed()); //Not fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(5));
+ auto F5 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(5));
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor