diff --git a/CMakeLists.txt b/CMakeLists.txt
index cd0c6487988fb0435b6593e8e96a3c9c034320cc..f3eb18d8063ef9182c7e614f4ab22ca33159459c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -45,12 +45,15 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
+<<<<<<< HEAD
if(UNIX)
# GCC is not strict enough by default, so enable most of the warnings.
set(CMAKE_CXX_FLAGS
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers -Wno-unused-but-set-variable")
endif(UNIX)
+=======
+>>>>>>> devel
#Set compiler according C++11 support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
@@ -65,6 +68,12 @@ else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
+if(UNIX)
+ # GCC is not strict enough by default, so enable most of the warnings.
+ set(CMAKE_CXX_FLAGS
+ "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+endif(UNIX)
+
#OPTION(BUILD_DOC "Build Documentation" OFF)
OPTION(BUILD_TESTS "Build Unit tests" ON)
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 3708f51d3aa1141756dc54c27f76039671bfa42e..c73e295083b71c0237d2569ff90ff0b9191b1192 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -225,7 +225,7 @@ int main()
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKey())
+ if (kf->isKeyOrAux())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
@@ -245,7 +245,7 @@ int main()
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList())
- if (kf->isKey())
+ if (kf->isKeyOrAux())
{
Eigen::MatrixXs cov;
kf->getCovariance(cov);
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
deleted file mode 100644
index 49f3669bf4dbc0c0c1b145eb92687ea6c3506f09..0000000000000000000000000000000000000000
--- a/include/base/ceres_wrapper/solver_manager.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef SOLVER_MANAGER_H_
-#define SOLVER_MANAGER_H_
-
-//wolf includes
-#include "base/common/wolf.h"
-#include "base/state_block/state_block.h"
-#include "base/factor/factor_base.h"
-
-namespace wolf {
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
-
-WOLF_PTR_TYPEDEFS(SolverManager);
-
-/** \brief Solver manager for WOLF
- *
- */
-
-class SolverManager
-{
- protected:
- ProblemPtr wolf_problem_;
-
- public:
- SolverManager(ProblemPtr _wolf_problem);
-
- virtual ~SolverManager();
-
- virtual std::string solve(const unsigned int& _report_level) = 0;
-
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
-
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
-
- virtual void update();
-
- virtual ProblemPtr getProblem();
-
- private:
-
- virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
-
- virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
-
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h
index ffa84d74d0c7521ce226a5445e58591bfc43be66..f4befb4d11cbc5f54a093e7a6ebcfa221c72b275 100644
--- a/include/base/frame/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -18,12 +18,13 @@ class StateBlock;
namespace wolf {
-/** \brief Enumeration of frame types: key-frame or non-key-frame
+/** \brief Enumeration of frame types
*/
typedef enum
{
- NON_KEY_FRAME = 0, ///< Regular frame. It does not play at optimization.
- KEY_FRAME = 1 ///< key frame. It plays at optimizations.
+ KEY = 2, ///< key frame. It plays at optimizations (estimated).
+ AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
+ NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
} FrameType;
//class FrameBase
@@ -71,9 +72,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
public:
unsigned int id();
- // KeyFrame / NonKeyFrame
+ // get type
bool isKey() const;
+ bool isAux() const;
+ bool isKeyOrAux() const;
+
+ // set type
void setKey();
+ void setAux();
// Frame values ------------------------------------------------
public:
@@ -169,7 +175,17 @@ inline unsigned int FrameBase::id()
inline bool FrameBase::isKey() const
{
- return (type_ == KEY_FRAME);
+ return (type_ == KEY);
+}
+
+inline bool FrameBase::isAux() const
+{
+ return (type_ == AUXILIARY);
+}
+
+inline bool FrameBase::isKeyOrAux() const
+{
+ return (type_ == KEY || type_ == AUXILIARY);
}
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index 938074f78020ebff96905714d70d7f1259a168e1..9c1d768e0d1be2fbac9cf2c9d926e69d8769564e 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -3,6 +3,7 @@
// Fwd refs
namespace wolf{
+class SolverManager;
class HardwareBase;
class TrajectoryBase;
class MapBase;
@@ -33,6 +34,7 @@ enum Notification
*/
class Problem : public std::enable_shared_from_this<Problem>
{
+ friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
protected:
HardwareBasePtr hardware_ptr_;
@@ -138,7 +140,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Emplace frame from string frame_structure
* \param _frame_structure String indicating the frame structure.
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
*
@@ -154,7 +156,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Emplace frame from string frame_structure without state
* \param _frame_structure String indicating the frame structure.
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -167,7 +169,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without structure
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
*
@@ -181,7 +183,7 @@ class Problem : public std::enable_shared_from_this<Problem>
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without structure nor state
- * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -193,26 +195,44 @@ class Problem : public std::enable_shared_from_this<Problem>
const TimeStamp& _time_stamp);
// Frame getters
- FrameBasePtr getLastFrame ( );
- FrameBasePtr getLastKeyFrame ( );
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
+ FrameBasePtr getLastFrame( ) const;
+ FrameBasePtr getLastKeyFrame( ) const;
+ FrameBasePtr getLastKeyOrAuxFrame( ) const;
+ FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
- /** \brief Give the permission to a processor to create a new keyFrame
+ /** \brief Give the permission to a processor to create a new key Frame
*
- * This should implement a black list of processors that have forbidden keyframe creation.
+ * This should implement a black list of processors that have forbidden key frame creation.
* - This decision is made at problem level, not at processor configuration level.
- * - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
+ * - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
*/
bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
/** \brief New key frame callback
*
- * New key frame callback: It should be called by any processor that creates a new keyframe. It calls the keyFrameCallback of the rest of processors.
+ * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
ProcessorBasePtr _processor_ptr, //
const Scalar& _time_tolerance);
+ /** \brief Give the permission to a processor to create a new auxiliary Frame
+ *
+ * This should implement a black list of processors that have forbidden auxiliary frame creation.
+ * - This decision is made at problem level, not at processor configuration level.
+ * - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
+ */
+ bool permitAuxFrame(ProcessorBasePtr _processor_ptr);
+
+ /** \brief New auxiliary frame callback
+ *
+ * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
+ */
+ void auxFrameCallback(FrameBasePtr _frame_ptr, //
+ ProcessorBasePtr _processor_ptr, //
+ const Scalar& _time_tolerance);
+
// State getters
Eigen::VectorXs getCurrentState ( );
void getCurrentState (Eigen::VectorXs& state);
@@ -240,6 +260,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
+ bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance);
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
// Solver management ----------------------------------------
@@ -252,9 +273,13 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
+ /** \brief Returns the size of the map of state block notification
*/
- std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+ SizeStd getStateBlockNotificationMapSize() const;
+
+ /** \brief Returns if the state block has been notified, and the notification via parameter
+ */
+ bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
/** \brief Notifies a new factor to be added to the solver manager
*/
@@ -264,10 +289,24 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeFactor(FactorBasePtr _factor_ptr);
+ /** \brief Returns the size of the map of factor notification
+ */
+ SizeStd getFactorNotificationMapSize() const;
+
+ /** \brief Returns if the factor has been notified, and the notification via parameter
+ */
+ bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
+
+ protected:
+ /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+ */
+ std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+
/** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
+ public:
// Print and check ---------------------------------------
/**
* \brief print wolf tree
@@ -311,12 +350,25 @@ inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificati
return std::move(state_block_notification_map_);
}
+inline SizeStd Problem::getStateBlockNotificationMapSize() const
+{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ return state_block_notification_map_.size();
+}
+
inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
return std::move(factor_notification_map_);
}
+inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
+{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ return factor_notification_map_.size();
+}
+
+
} // namespace wolf
#endif // PROBLEM_H
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 5d68c43c7b921532dc1d1d0d32ddabe89d75de93..9e149abcbdfca7c94942b92a1669c9c8ee5f6e70 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -110,16 +110,19 @@ struct ProcessorParamsBase
ProcessorParamsBase() = default;
ProcessorParamsBase(bool _voting_active,
- Scalar _time_tolerance)
- : voting_active(_voting_active)
- , time_tolerance(_time_tolerance)
+ Scalar _time_tolerance,
+ bool _voting_aux_active = false) :
+ voting_active(_voting_active),
+ voting_aux_active(_voting_aux_active),
+ time_tolerance(_time_tolerance)
{
//
}
virtual ~ProcessorParamsBase() = default;
- bool voting_active = false;
+ bool voting_active = false; ///< Whether this processor is allowed to vote for a Key Frame or not
+ bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
///< maximum time difference between a Keyframe time stamp and
/// a particular Capture of this processor to allow assigning
@@ -159,8 +162,19 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
virtual bool voteForKeyFrame() = 0;
+ /** \brief Vote for Auxiliary Frame generation
+ *
+ * If a Auxiliary Frame criterion is validated, this function returns true,
+ * meaning that it wants to create a Auxiliary Frame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create Auxiliary Frames!
+ */
+ virtual bool voteForAuxFrame(){return false;};
+
virtual bool permittedKeyFrame() final;
+ virtual bool permittedAuxFrame() final;
+
/**\brief make a Frame with the provided Capture
*
* Provide the following functionality:
@@ -192,7 +206,11 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool isVotingActive() const;
+ bool isVotingAuxActive() const;
+
void setVotingActive(bool _voting_active = true);
+
+ void setVotingAuxActive(bool _voting_active = true);
};
inline bool ProcessorBase::isVotingActive() const
@@ -200,11 +218,21 @@ inline bool ProcessorBase::isVotingActive() const
return params_->voting_active;
}
+inline bool ProcessorBase::isVotingAuxActive() const
+{
+ return params_->voting_aux_active;
+}
+
inline void ProcessorBase::setVotingActive(bool _voting_active)
{
params_->voting_active = _voting_active;
}
+inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
+{
+ params_->voting_aux_active = _voting_active;
+}
+
}
#include "base/sensor/sensor_base.h"
diff --git a/include/base/trajectory/trajectory_base.h b/include/base/trajectory/trajectory_base.h
index 37f9762c7c9955381eaba9bc52aef253441d36e1..aff013ea00d0b40af358ad4d3cb980d8298ccc4e 100644
--- a/include/base/trajectory/trajectory_base.h
+++ b/include/base/trajectory/trajectory_base.h
@@ -26,6 +26,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
protected:
std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
+ FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
public:
TrajectoryBase(const std::string& _frame_sturcture);
@@ -37,18 +38,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
// Frames
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
FrameBasePtrList& getFrameList();
- FrameBasePtr getLastFrame();
- FrameBasePtr getLastKeyFrame();
- FrameBasePtr findLastKeyFrame();
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
+ const FrameBasePtrList& getFrameList() const;
+ FrameBasePtr getLastFrame() const;
+ FrameBasePtr getLastKeyFrame() const;
+ FrameBasePtr getLastKeyOrAuxFrame() const;
+ FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
void sortFrame(FrameBasePtr _frm_ptr);
- void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
- FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
+ void updateLastFrames();
// factors
void getFactorList(FactorBasePtrList & _fac_list);
+ protected:
+ FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
+ void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
};
inline FrameBasePtrList& TrajectoryBase::getFrameList()
@@ -56,19 +60,24 @@ inline FrameBasePtrList& TrajectoryBase::getFrameList()
return frame_list_;
}
-inline FrameBasePtr TrajectoryBase::getLastFrame()
+inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+{
+ return frame_list_;
+}
+
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
{
return frame_list_.back();
}
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
+inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
{
return last_key_frame_ptr_;
}
-inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
+inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
{
- last_key_frame_ptr_ = _key_frame_ptr;
+ return last_key_or_aux_frame_ptr_;
}
inline std::string TrajectoryBase::getFrameStructure() const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0e071669bf7b2320e0ca58001aa4609becc4ab84..a938e36ee5b313d0c696d52b16ee589c0548413d 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -81,7 +81,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKey())
+ if (fr_ptr->isKeyOrAux())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
all_state_blocks.push_back(sb);
@@ -106,7 +106,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
{
// first create a vector containing all state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKey())
+ if (fr_ptr->isKeyOrAux())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
for(auto sb2 : fr_ptr->getStateBlockVec())
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index d12f491de7ce0895ca301498454e7f3d451bbd4e..456367a2c67b1c98ca91e9631c3f3da47751882a 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -247,7 +247,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
// Prior covariance
CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 136ba285e0d9fb2c62b4801dc75e597dcdd257ff..62ad277f98576b1bf17e17c7515d8dfb428c93be 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -254,7 +254,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
// Prior covariance
CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 3d0483e9647f9e3203ab3bb89036de55e0f9adc9..4040194b382e9db0698b2a5d8acf0f852af1286b 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
//create FrameIMU
ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+ FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
@@ -141,7 +141,7 @@ int main(int argc, char** argv)
//create FrameIMU
ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+ FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
@@ -199,7 +199,7 @@ int main(int argc, char** argv)
//create FrameIMU
ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+ FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index be7df536e971273559252aeff8dc51bd808bb14b..8baee2c1e75af6ab5a8841e85b082d8f7cdd20c9 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -98,7 +98,7 @@ int main(int argc, char** argv)
//=====================================================
// Origin Key Frame is fixed
TimeStamp t = 0;
- FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
+ FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
problem->getProcessorMotion()->setOrigin(origin_frame);
origin_frame->fix();
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4..f07a2c1ce3da6eb7168571d3ec1377ffa8b0c388 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -97,11 +97,11 @@ int main(int argc, char** argv)
/* 1 */
ProblemPtr problem = Problem::create("PO 3D");
// One anchor frame to define the lmk, and a copy to make a factor
- FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
// and two other frames to observe the lmk
- FrameBasePtr kf_3 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
kf_1->fix();
kf_2->fix();
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 56982c9dc7c6fda44b594a04f92a33f911b8ca49..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -31,22 +31,22 @@ int main()
{
ProblemPtr problem_ptr = Problem::create(FRM_PO_2D);
- problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
- FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
- FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
- problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
- FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
- FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
+ problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
+ FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
+ FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
+ problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
+ FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
+ FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
printFrames(problem_ptr);
std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl;
- frm5->setKey();
+ frm5->setEstimated();
printFrames(problem_ptr);
std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl;
- frm2->setKey();
+ frm2->setEstimated();
printFrames(problem_ptr);
@@ -56,21 +56,21 @@ int main()
printFrames(problem_ptr);
std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl;
- frm3->setKey();
+ frm3->setEstimated();
printFrames(problem_ptr);
std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl;
- frm6->setKey();
+ frm6->setEstimated();
printFrames(problem_ptr);
- FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
+ FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
printFrames(problem_ptr);
- FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
+ FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
printFrames(problem_ptr);
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index 7b9e85c085a04dc401097db87be3dcc8ea25e17d..03d214a3d09c8a624129243f181aed1dd722b5a9 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -216,7 +216,7 @@ int main(int argc, char** argv)
// ------------------------ START EXPERIMENT ------------------------
// First frame FIXED
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0));
+ last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0));
last_frame_ptr->fix();
bl_problem_ptr->print(4, true, false, true);
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
Eigen::Vector3s from_pose = frame_from_ptr->getState();
R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
Eigen::Vector3s new_frame_pose = from_pose + R*meas;
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to)));
+ last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
frame_to_ptr = last_frame_ptr;
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index 1113af8b7f7e9bd40033ed60e182cc2f485eee99..40171695f14ac3ef2aeee7091ca53e8de50a2a90 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_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_FRAME, 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_FRAME, 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(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)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index f3ed29007190ada48bba1758a1ac4e57843ff423..b4d9ac23316dc5450b269d99ce45cbb0f7c0e2f0 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -148,8 +148,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, 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_FRAME, 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(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)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index cb7f089c1f21adad761f5fd7c13f0eee786b6741..10c75405b377474905b1040251794067d7df115c 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -16,7 +16,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
trajectory_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
- type_(NON_KEY_FRAME),
+ type_(NON_ESTIMATED),
time_stamp_(_ts)
{
state_block_vec_[0] = _p_ptr;
@@ -39,8 +39,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
FrameBase::~FrameBase()
{
- if ( isKey() )
- removeStateBlocks();
}
void FrameBase::remove()
@@ -68,11 +66,12 @@ void FrameBase::remove()
}
// Remove Frame State Blocks
- if ( isKey() )
+ if ( isKeyOrAux() )
removeStateBlocks();
- if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
- getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
+
+ if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id())
+ getTrajectory()->updateLastFrames();
// std::cout << "Removed F" << id() << std::endl;
}
@@ -81,7 +80,7 @@ void FrameBase::remove()
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
- if (isKey() && getTrajectory() != nullptr)
+ if (isKeyOrAux() && getTrajectory() != nullptr)
getTrajectory()->sortFrame(shared_from_this());
}
@@ -113,18 +112,25 @@ void FrameBase::removeStateBlocks()
void FrameBase::setKey()
{
- if (type_ != KEY_FRAME)
- {
- type_ = KEY_FRAME;
+ // register if previously not estimated
+ if (!isKeyOrAux())
registerNewStateBlocks();
- if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
- getTrajectory()->setLastKeyFrame(shared_from_this());
+ // WOLF_DEBUG("Set Key", this->id());
+ type_ = KEY;
+ getTrajectory()->sortFrame(shared_from_this());
+ getTrajectory()->updateLastFrames();
+}
- getTrajectory()->sortFrame(shared_from_this());
+void FrameBase::setAux()
+{
+ if (!isKeyOrAux())
+ registerNewStateBlocks();
-// WOLF_DEBUG("Set KF", this->id());
- }
+ // WOLF_DEBUG("Set Auxiliary", this->id());
+ type_ = AUXILIARY;
+ getTrajectory()->sortFrame(shared_from_this());
+ getTrajectory()->updateLastFrames();
}
void FrameBase::fix()
@@ -170,7 +176,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
for (StateBlockPtr sb : state_block_vec_)
if (sb && (index < _st_size))
{
- sb->setState(_state.segment(index, sb->getSize()), isKey());
+ sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 3b476f4fd9e13390160adf993d19f900f8233f3c..321ec061fdba45b4e71f3da8bc3941790f02c21e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -251,8 +251,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
{
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
- else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
- trajectory_ptr_->getLastKeyFrame()->getState(state);
+ else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+ trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
else
state = zeroState();
}
@@ -264,10 +264,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
processor_motion_ptr_->getCurrentState(state);
processor_motion_ptr_->getCurrentTimeStamp(ts);
}
- else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
+ else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
{
- trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
- trajectory_ptr_->getLastKeyFrame()->getState(state);
+ trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
+ trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
}
else
state = zeroState();
@@ -278,7 +278,7 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
{
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+ FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
if (closest_frame != nullptr)
closest_frame->getState(state);
else
@@ -348,6 +348,30 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
}
+bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr)
+{
+ // This should implement a black list of processors that have forbidden auxiliary frame creation
+ // This decision is made at problem level, not at processor configuration level.
+ // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
+
+ // Currently allowing all processors to vote:
+ return true;
+}
+
+void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
+{
+ if (_processor_ptr)
+ {
+ WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+ }
+ else
+ {
+ WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+ }
+
+ processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+}
+
LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
{
getMap()->addLandmark(_lmk_ptr);
@@ -401,6 +425,16 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
state_block_notification_map_[_state_ptr] = REMOVE;
}
+bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
+{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
+ return false;
+
+ notif = state_block_notification_map_.at(sb_ptr);
+ return true;
+}
+
FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
@@ -442,6 +476,16 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
factor_notification_map_[_factor_ptr] = REMOVE;
}
+bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
+{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
+ return false;
+
+ notif = factor_notification_map_.at(fac_ptr);
+ return true;
+}
+
void Problem::clearCovariance()
{
std::lock_guard<std::mutex> lock(mut_covariances_);
@@ -587,6 +631,12 @@ bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
return getFrameCovariance(frm, cov);
}
+bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov)
+{
+ FrameBasePtr frm = getLastKeyOrAuxFrame();
+ return getFrameCovariance(frm, cov);
+}
+
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
{
bool success(true);
@@ -639,22 +689,37 @@ HardwareBasePtr Problem::getHardware()
return hardware_ptr_;
}
-FrameBasePtr Problem::getLastFrame()
+FrameBasePtr Problem::getLastFrame() const
{
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFrame()
+FrameBasePtr Problem::getLastKeyFrame() const
{
return trajectory_ptr_->getLastKeyFrame();
}
+FrameBasePtr Problem::getLastKeyOrAuxFrame() const
+{
+ return trajectory_ptr_->getLastKeyOrAuxFrame();
+}
+
+FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+}
+
+FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+}
+
FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
if ( ! prior_is_set_ )
{
// Create origin frame
- FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts);
+ FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
// create origin capture with the given state as data
// Capture fix only takes 3D position and Quaternion orientation
@@ -765,10 +830,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
std::cout << " pm" << p->id() << " " << p->getType() << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
<< pm->getOrigin()->getFrame()->id() << endl;
if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? " KF" : " F")
+ cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
<< pm->getLast()->getFrame()->id() << endl;
if (pm->getIncoming())
cout << " i: C" << pm->getIncoming()->id() << endl;
@@ -783,14 +848,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// if (ptt)
// {
// if (ptt->getPrevOrigin())
-// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " F")
+// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
// << ptt->getPrevOrigin()->getFrame()->id() << endl;
// }
if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
<< pt->getOrigin()->getFrame()->id() << endl;
if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? " KF" : " F")
+ cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
<< pt->getLast()->getFrame()->id() << endl;
if (pt->getIncoming())
cout << " i: C" << pt->getIncoming()->id() << endl;
@@ -806,7 +871,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
{
cout << " <-- ";
@@ -963,11 +1028,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
}
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
-{
- return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
bool Problem::check(int verbose_level)
{
using std::cout;
@@ -1047,7 +1107,7 @@ bool Problem::check(int verbose_level)
{
if (verbose_level > 0)
{
- cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl;
+ cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl;
cout << " -> P @ " << F->getProblem().get() << endl;
cout << " -> T @ " << F->getTrajectory().get() << endl;
for (auto sb : F->getStateBlockVec())
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 761b7729d058233bfc0750ed69fe70c8ed219144..41dfe959093847801a1fd8ad487923e97c981ffe 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -36,8 +36,6 @@ ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const Pro
bool ProcessorIMU::voteForKeyFrame()
{
- if(!isVotingActive())
- return false;
// time span
if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
{
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 0679909164d86a96ac609a930619f35ff6a97fe3..77e00b83580074483b286457655c8b08edf534d3 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -26,9 +26,14 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
+bool ProcessorBase::permittedAuxFrame()
+{
+ return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this());
+}
+
FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr)
{
- std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl;
+ std::cout << "Making " << (_type == KEY ? "key-" : (_type == AUXILIARY ? "aux-" : "")) << "frame" << std::endl;
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
new_frame_ptr->addCapture(_capture_ptr);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 66b4616fc442045e9b5c3ca4650b73707773f7e3..1206d10fcae85f653ff6c523a01de30fb1d3f5a3 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -209,7 +209,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
last_ptr_->getFrame()->setState(getCurrentState());
- if (voteForKeyFrame() && permittedKeyFrame())
+ if (permittedKeyFrame() && voteForKeyFrame())
{
// Set the frame of last_ptr as key
auto key_frame_ptr = last_ptr_->getFrame();
@@ -222,7 +222,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
// create a new frame
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
+ auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
getCurrentState(),
getCurrentTimeStamp());
// create a new capture
@@ -303,7 +303,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin);
+ FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -329,7 +329,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
+ auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
_origin_frame->getState(),
_origin_frame->getTimeStamp());
// emplace (emtpy) last Capture
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index d2ff8055c15f6573f7e7d1e0b915f27f91fea702..9c6e0d6c93fca62476364d1f8744fcc9eaf01576 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -75,7 +75,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case FIRST_TIME_WITHOUT_PACK :
{
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
+ FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
kfrm->addCapture(incoming_ptr_);
// Process info
@@ -101,7 +101,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
} // @suppress("No break at end of case")
case SECOND_TIME_WITHOUT_PACK :
{
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
@@ -137,7 +137,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
pack->key_frame->addCapture(last_ptr_);
// Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -174,15 +174,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
last_ptr_->getFrame()->setKey();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// process
processNew(params_tracker_->max_new_features);
- // // Set key
- // last_ptr_->getFrame()->setKey();
- //
// Set state to the keyframe
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
@@ -199,6 +196,38 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
incoming_ptr_ = nullptr;
}
+ /* TODO: create an auxiliary frame
+ else if (voteForAuxFrame() && permittedAuxFrame())
+ {
+ // We create an Auxiliary Frame
+
+ // set AuxF on last
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+ last_ptr_->getFrame()->setAuxiliary();
+
+ // make F; append incoming to new F
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ frm->addCapture(incoming_ptr_);
+
+ // Set state to the keyframe
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+
+ // Establish factors
+ establishFactors();
+
+ // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors
+ getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
+
+ // Advance this
+ last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+ // do not remove! last_ptr_->remove();
+ incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
+
+ // Update pointers
+ advanceDerived();
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ }*/
else
{
// We do not create a KF
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index ce169fed8a58367867495db130ef29daba23ddb2..e28e18cdea2cc5c8cfd88d55a2d311d81a22a6b0 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -6,7 +6,8 @@ namespace wolf {
TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
NodeBase("TRAJECTORY", "Base"),
frame_structure_(_frame_structure),
- last_key_frame_ptr_(nullptr)
+ last_key_frame_ptr_(nullptr),
+ last_key_or_aux_frame_ptr_(nullptr)
{
// WOLF_DEBUG("constructed T");
}
@@ -22,17 +23,19 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
_frame_ptr->setTrajectory(shared_from_this());
_frame_ptr->setProblem(getProblem());
- if (_frame_ptr->isKey())
+ // add to list
+ frame_list_.push_back(_frame_ptr);
+
+ if (_frame_ptr->isKeyOrAux())
{
+ // register state blocks
_frame_ptr->registerNewStateBlocks();
- if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
- last_key_frame_ptr_ = _frame_ptr;
- frame_list_.insert(computeFrameOrder(_frame_ptr), _frame_ptr);
- }
- else
- {
- frame_list_.push_back(_frame_ptr);
+ // sort
+ sortFrame(_frame_ptr);
+
+ // update last_estimated and last_key
+ updateLastFrames();
}
return _frame_ptr;
@@ -47,7 +50,6 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
{
moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
- // last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above
}
void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
@@ -56,34 +58,46 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
{
frame_list_.remove(_frm_ptr);
frame_list_.insert(_place, _frm_ptr);
- last_key_frame_ptr_ = findLastKeyFrame();
+ updateLastFrames();
}
}
FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
{
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
+ if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
return frm_rit.base();
return getFrameList().begin();
}
-FrameBasePtr TrajectoryBase::findLastKeyFrame()
+void TrajectoryBase::updateLastFrames()
{
- // NOTE: Assumes keyframes are sorted by timestamp
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
- if ((*frm_rit)->isKey())
- return (*frm_rit);
+ bool last_estimated_set = false;
- return nullptr;
+ // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
+ for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
+ if ((*frm_rit)->isKeyOrAux())
+ {
+ if (!last_estimated_set)
+ {
+ last_key_or_aux_frame_ptr_ = (*frm_rit);
+ last_estimated_set = true;
+ }
+ if ((*frm_rit)->isKey())
+ {
+ last_key_frame_ptr_ = (*frm_rit);
+ break;
+ }
+ }
}
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
+FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
{
+ // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
FrameBasePtr closest_kf = nullptr;
Scalar min_dt = 1e9;
-
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
+
+ for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
if ((*frm_rit)->isKey())
{
Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
@@ -98,4 +112,25 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
return closest_kf;
}
+FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
+ FrameBasePtr closest_kf = nullptr;
+ Scalar min_dt = 1e9;
+
+ for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
+ if ((*frm_rit)->isKeyOrAux())
+ {
+ Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
+ if (dt < min_dt)
+ {
+ min_dt = dt;
+ closest_kf = *frm_rit;
+ }
+ else
+ break;
+ }
+ return closest_kf;
+}
+
} // namespace wolf
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index d2657e4156cf6991d3342505cb558712837cb344..253c7da0cba0aebdcbff3d30521b9511987d075f 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -480,7 +480,7 @@ class Process_Factor_IMU : public testing::Test
virtual void buildProblem()
{
// ===================================== SET KF in Wolf tree
- FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t);
+ FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t);
// ===================================== IMU CALLBACK
problem->keyFrameCallback(KF, nullptr, dt/2);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 9b096f7c82d24aa1b2b939919f46a5190f8a65c0..3c5f2502cbce65184e2d63d59b85145776a86f5c 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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -401,7 +401,7 @@ TEST(CeresManager, AddRemoveFactor)
// remove factor
P->removeFactor(c);
- ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // add+remove = empty
+ ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
// update solver
ceres_manager_ptr->update();
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
@@ -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_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 204c6ce633a36380d46eacbc65d66abe1a7747fe..fd9650de70d970b9336d62e8471f3c5a090c0221 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV 3D");
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
// Capture, feature and factor
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("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 5c508f89d0de0ebddc7e4ce306dddf9aeee5efed..f22c65a51ed5b22cbf57f22aafbc6a81ee52e281 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 3D");
ceres_manager = std::make_shared<CeresManager>(problem);
- F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0);
+ F1 = problem->emplaceFrame (KEY, pose1, 1.0);
- F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0);
+ F2 = problem->emplaceFrame (KEY, pose2, 2.0);
C2 = std::make_shared<CaptureBase>("Base", 1.0);
F2->addCapture(C2);
f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 98af13f6e834f73bd4bbab7e524cf8928d0467af..78223c6020f9241c9a4faa7d24db5346c06e9c6d 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -133,19 +133,19 @@ class FactorAutodiffTrifocalTest : public testing::Test{
Vector2s pix(0,0);
Matrix2s pix_cov; pix_cov.setIdentity();
- F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0);
+ F1 = problem->emplaceFrame(KEY, pose1, 1.0);
I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
F1-> addCapture(I1);
f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I1-> addFeature(f1);
- F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0);
+ F2 = problem->emplaceFrame(KEY, pose2, 2.0);
I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1));
F2-> addCapture(I2);
f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I2-> addFeature(f2);
- F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0);
+ F3 = problem->emplaceFrame(KEY, pose3, 3.0);
I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1));
F3-> addCapture(I3);
f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index c3f767d56ea04682d2e33651c4e4e1a89f865585..2b8a6d20160d9464ffa7b48ff9044b66b03d50d9 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 3D");
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index d7bce0c84f734613f1a2f143bebf2690897149f2..d07b564a3caeed5dd34bb9e107776bc769f3d85d 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO 2D");
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor from frm1 to frm0
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..620e389c6b5a483f84250584cbe6d318e62ec55b 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO 3D");
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 082e438d0b799e1ed32075f28e6f61b7450da8e0..c9d894e9ff3a8fafdee13087ef7364bf7173277f 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -77,7 +77,7 @@ class FeatureIMU_test : public testing::Test
//create Frame
ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
state_vec = problem->getProcessorMotion()->getCurrentState();
- last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
+ last_frame = std::make_shared<FrameBase>(KEY, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
problem->getTrajectory()->addFrame(last_frame);
//create a feature
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 9b4c7b6cf882b1f8104fa71ac3d60e12d8580137..9984c258de1026f68debd1cf4ccd072d4cdbd3c5 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -32,6 +32,7 @@ TEST(FrameBase, GettersAndSetters)
ASSERT_EQ(t, 1);
ASSERT_FALSE(F->isFixed());
ASSERT_EQ(F->isKey(), false);
+ ASSERT_EQ(F->isKeyOrAux(), false);
}
TEST(FrameBase, StateBlocks)
@@ -123,6 +124,7 @@ TEST(FrameBase, LinksToTree)
// set key
F1->setKey();
ASSERT_TRUE(F1->isKey());
+ ASSERT_TRUE(F1->isKeyOrAux());
// Unlink
F1->unlinkCapture(C);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 604ef489478f5779ac4b79a14095c37436c81be4..feafb83e98ce51caf43aea88d2a192b00fff78e8 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -131,7 +131,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
+ FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
@@ -139,7 +139,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
+ FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
@@ -404,7 +404,7 @@ TEST(Odom2D, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
Vector3s x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -436,7 +436,7 @@ TEST(Odom2D, KF_callback)
problem->print(4,1,1,1);
x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, 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 b65bea49219ff2fa5e2c29233d3e4d21a8e35704..143b66549f99b4d385bb3f61c5250a19979be257 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -9,6 +9,7 @@
#include "base/utils/logging.h"
#include "base/problem/problem.h"
+#include "base/solver/solver_manager.h"
#include "base/sensor/sensor_base.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
@@ -19,11 +20,36 @@
using namespace wolf;
using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(DummySolverManager);
+
+struct DummySolverManager : public SolverManager
+{
+ DummySolverManager(const ProblemPtr& _problem)
+ : SolverManager(_problem)
+ {
+ //
+ }
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+ virtual bool hasConverged(){return true;};
+ virtual SizeStd iterations(){return 0;};
+ virtual Scalar initialCost(){return 0;};
+ virtual Scalar finalCost(){return 0;};
+ virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
+ virtual void addFactor(const FactorBasePtr& fac_ptr){};
+ virtual void removeFactor(const FactorBasePtr& fac_ptr){};
+ virtual void addStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
+};
+
TEST(Problem, create)
{
ProblemPtr P = Problem::create("POV 3D");
- // check double ointers to branches
+ // check double pointers to branches
ASSERT_EQ(P, P->getHardware()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getMap()->getProblem());
@@ -109,20 +135,20 @@ TEST(Problem, SetOrigin_PO_2D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
@@ -130,8 +156,8 @@ TEST(Problem, SetOrigin_PO_2D)
ASSERT_FALSE(c->getLandmarkOther());
// check that the Feature measurement and covariance are the ones provided
- ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -146,20 +172,20 @@ TEST(Problem, SetOrigin_PO_3D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
@@ -177,9 +203,9 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO 2D");
- FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY_FRAME, VectorXs(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY_FRAME, VectorXs(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY_FRAME, VectorXs(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY, VectorXs(3), TimeStamp(0.0));
+ FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY, VectorXs(7), TimeStamp(1.0));
+ FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY, VectorXs(10), TimeStamp(2.0));
// std::cout << "f0: type PO 2D? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3D? " << f1->getType() << std::endl;
@@ -190,7 +216,7 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
@@ -207,11 +233,11 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*2 + */3)); // consume empties the notification map, so only should contain notification since last call
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 3));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
@@ -223,16 +249,40 @@ TEST(Problem, StateBlocks)
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// 2 state blocks, estimated
- P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
+ auto KF = P->emplaceFrame("PO 3D", KEY, xs, 0);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2));
+
+ // Notifications
+ Notification notif;
+ ASSERT_TRUE(P->getStateBlockNotification(KF->getP(), notif));
+ ASSERT_EQ(notif, ADD);
+ ASSERT_TRUE(P->getStateBlockNotification(KF->getO(), notif));
+ ASSERT_EQ(notif, ADD);
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// P->print(4,1,1,1);
+
+ // consume notifications
+ DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2));
+ SM->update(); // calls P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
+
+ // remove frame
+ auto SB_P = KF->getP();
+ auto SB_O = KF->getO();
+ KF->remove();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
+ ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
+ ASSERT_EQ(notif, REMOVE);
+ ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
+ ASSERT_EQ(notif, REMOVE);
+
}
TEST(Problem, Covariances)
@@ -256,7 +306,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+ FrameBasePtr F = P->emplaceFrame("PO 3D", KEY, xs, 0);
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 904dfd7646f1194892e0845b003cd20d17916c4b..34128f0db287f152a36074fe16b2e61db9a37153 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -59,7 +59,7 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
- // Sequence to test KeyFrame creations (callback calls)
+ // Sequence to test Key Frame creations (callback calls)
// initialize
TimeStamp t(0.0);
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index cfbdd69e0f978d8822bf0e707ff48a2a3fcefbbd..75a0cdefbb70da4d75edb2d1e4a7e5d79d13a29c 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -72,19 +72,19 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
// create Keyframes
- F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+ F1 = std::make_shared<wolf::FrameBase>(wolf::KEY,
1,
stateblock_pptr1,
stateblock_optr1);
- F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+ F2 = std::make_shared<wolf::FrameBase>(wolf::KEY,
1,
stateblock_pptr2,
stateblock_optr2);
- F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+ F3 = std::make_shared<wolf::FrameBase>(wolf::KEY,
1,
stateblock_pptr3,
stateblock_optr3);
- F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+ F4 = std::make_shared<wolf::FrameBase>(wolf::KEY,
1,
stateblock_pptr4,
stateblock_optr4);
@@ -147,8 +147,9 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
1.2,
sensor_ptr);
- // Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F3);
+ // Make 3rd frame last Key frame
+ F3->setTimeStamp(wolf::TimeStamp(2.0));
+ problem->getTrajectory()->sortFrame(F3);
// trigger search for loopclosure
processor_ptr_point2d->process(incomming_dummy);
@@ -180,8 +181,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
problem->addCovarianceBlock( F4->getP(), F4->getP(),
position_covariance_matrix);
- // Make 3rd frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F3);
+ // Make 3rd frame last Key frame
+ F3->setTimeStamp(wolf::TimeStamp(2.0));
+ problem->getTrajectory()->sortFrame(F3);
// trigger search for loopclosure
processor_ptr_ellipse2d->process(incomming_dummy);
@@ -192,8 +194,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
- // Make 4th frame last Keyframe
- problem->getTrajectory()->setLastKeyFrame(F4);
+ // Make 4th frame last Key frame
+ F4->setTimeStamp(wolf::TimeStamp(3.0));
+ problem->getTrajectory()->sortFrame(F4);
// trigger search for loopclosure again
processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 1928d5a30fc7cd7c5a63cfcefb684b17fa675709..91ec5dba639cc2a89dc99a94de23e5483adf981b 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -450,9 +450,10 @@ TEST(SolverManager, AddUpdatedStateBlock)
// add state_block
P->addStateBlock(sb_ptr);
- auto state_block_notification_map = P->consumeStateBlockNotificationMap();
- ASSERT_EQ(state_block_notification_map.size(),1);
- ASSERT_EQ(state_block_notification_map.begin()->second,ADD);
+ Notification notif;
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
+ ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ ASSERT_EQ(notif, ADD);
// == Insert OTHER notifications ==
@@ -463,21 +464,25 @@ TEST(SolverManager, AddUpdatedStateBlock)
// Fix --> FLAG
sb_ptr->unfix();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // No new notifications (fix and set state are flags in sb)
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->removeStateBlock(sb_ptr);
- state_block_notification_map = P->consumeStateBlockNotificationMap();
- ASSERT_EQ(state_block_notification_map.size(),1);
- ASSERT_EQ(state_block_notification_map.begin()->second,REMOVE);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
+ ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ ASSERT_EQ(notif, REMOVE);
// == ADD + REMOVE: notification map should be empty ==
P->addStateBlock(sb_ptr);
P->removeStateBlock(sb_ptr);
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty());
+ ASSERT_TRUE(P->getStateBlockNotificationMapSize() == 0);
// == UPDATES should clear the list of notifications ==
// add state_block
@@ -486,7 +491,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
// update solver
solver_manager_ptr->update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // After solver_manager->update, notifications should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
}
TEST(SolverManager, AddFactor)
@@ -499,11 +504,16 @@ TEST(SolverManager, AddFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, ADD);
+
// update solver
solver_manager_ptr->update();
@@ -521,7 +531,7 @@ TEST(SolverManager, RemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -532,6 +542,11 @@ TEST(SolverManager, RemoveFactor)
// add factor
P->removeFactor(c);
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, REMOVE);
+
// update solver
solver_manager_ptr->update();
@@ -549,15 +564,22 @@ TEST(SolverManager, AddRemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, ADD);
+
// add factor
P->removeFactor(c);
- ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // ADD+REMOVE = empty
+ // notification
+ ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
+ ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
// update solver
solver_manager_ptr->update();
@@ -576,7 +598,7 @@ TEST(SolverManager, DoubleRemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index de582e26b0e3fbfa6ca61876e82d3b9870ea0ff8..1cd7c1a201806e04b59d56b33b83ab2681aa4e1f 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -9,6 +9,7 @@
#include "base/utils/logging.h"
#include "base/problem/problem.h"
+#include "base/solver/solver_manager.h"
#include "base/trajectory/trajectory_base.h"
#include "base/frame/frame_base.h"
@@ -16,45 +17,26 @@
using namespace wolf;
-struct DummyNotificationProcessor
+struct DummySolverManager : public SolverManager
{
- DummyNotificationProcessor(ProblemPtr _problem)
- : problem_(_problem)
+ DummySolverManager(const ProblemPtr& _problem)
+ : SolverManager(_problem)
{
//
}
-
- void update()
- {
- if (problem_ == nullptr)
- {
- FAIL() << "problem_ is nullptr !";
- }
-
- auto sb_noti_map = problem_->consumeStateBlockNotificationMap();
- ASSERT_TRUE(problem_->consumeStateBlockNotificationMap().empty()); // consume should empty the notification map
-
- while (!sb_noti_map.empty())
- {
- switch (sb_noti_map.begin()->second)
- {
- case ADD:
- {
- break;
- }
- case REMOVE:
- {
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
- }
- sb_noti_map.erase(sb_noti_map.begin());
- }
- ASSERT_TRUE(sb_noti_map.empty());
- }
-
- ProblemPtr problem_;
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+ virtual bool hasConverged(){return true;};
+ virtual SizeStd iterations(){return 0;};
+ virtual Scalar initialCost(){return 0;};
+ virtual Scalar finalCost(){return 0;};
+ virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
+ virtual void addFactor(const FactorBasePtr& fac_ptr){};
+ virtual void removeFactor(const FactorBasePtr& fac_ptr){};
+ virtual void addStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
};
/// Set to true if you want debug info
@@ -67,33 +49,74 @@ TEST(TrajectoryBase, ClosestKeyFrame)
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
- // kf1 kf2 f3 frames
+ // KF1 KF2 AuxF3 F4 frames
+ // 1 2 3 4 time stamps
+ // --+-----+-----+-----+---> time
+
+ FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr);
+ FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, nullptr, nullptr);
+ FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY, 3, nullptr, nullptr);
+ FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr);
+ T->addFrame(F1);
+ T->addFrame(F2);
+ T->addFrame(F3);
+ T->addFrame(F4);
+
+ FrameBasePtr KF; // closest key-frame queried
+
+ KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
+
+ KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
+
+ KF = T->closestKeyFrameToTimeStamp(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
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
+
+ KF = T->closestKeyFrameToTimeStamp(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
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
+}
+
+TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
+{
+
+ ProblemPtr P = Problem::create("PO 2D");
+ TrajectoryBasePtr T = P->getTrajectory();
+
+ // Trajectory status:
+ // KF1 KF2 F3 frames
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr);
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr);
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr);
- T->addFrame(f1);
- T->addFrame(f2);
- T->addFrame(f3);
+ FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr);
+ FrameBasePtr F2 = std::make_shared<FrameBase>(AUXILIARY, 2, nullptr, nullptr);
+ FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr);
+ T->addFrame(F1);
+ T->addFrame(F2);
+ T->addFrame(F3);
- FrameBasePtr kf; // closest key-frame queried
+ FrameBasePtr KF; // closest key-frame queried
- kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0
- ASSERT_EQ(kf->id(), f1->id()); // same id!
+ KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
- kf = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return f1
- ASSERT_EQ(kf->id(), f1->id()); // same id!
+ KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
- kf = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
+ KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
- kf = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
+ KF = T->closestKeyOrAuxFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
- kf = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return f2
- ASSERT_EQ(kf->id(), f2->id()); // same id!
+ KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
}
TEST(TrajectoryBase, Add_Remove_Frame)
@@ -103,71 +126,72 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectory();
- DummyNotificationProcessor N(P);
+ DummySolverManager N(P);
// Trajectory status:
- // kf1 kf2 f3 frames
+ // KF1 KF2 F3 frames
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+ FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+ FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
std::cout << __LINE__ << std::endl;
// add frames and keyframes
- T->addFrame(f1); // KF
+ T->addFrame(F1); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- T->addFrame(f2); // KF
+ T->addFrame(F2); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- T->addFrame(f3); // F (not KF so state blocks are not notified)
+ T->addFrame(F3); // F (not KF so state blocks are not notified)
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
// remove frames and keyframes
- f2->remove(); // KF
+ F2->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
std::cout << __LINE__ << std::endl;
- f3->remove(); // F
+ F3->remove(); // F
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
- f1->remove(); // KF
+ F1->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0);
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
}
@@ -179,70 +203,109 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
- // kf1 kf2 f3 frames
+ // KF1 KF2 F3 frames
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+ FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+ FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
// add frames and keyframes in random order --> keyframes must be sorted after that
- T->addFrame(f2); // KF2
+ T->addFrame(F2); // KF2
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- T->addFrame(f3); // F3
+ T->addFrame(F3); // F3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- T->addFrame(f1); // KF1
+ T->addFrame(F1); // KF1
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+ 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(); // set KF3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
- T->addFrame(f4);
+ FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ T->addFrame(F4);
// Trajectory status:
- // kf1 kf2 kf3 f4 frames
+ // KF1 KF2 KF3 F4 frames
// 1 2 3 1.5 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f4->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F4->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- f4->setKey();
+ F4->setKey();
// Trajectory status:
- // kf1 kf4 kf2 kf3 frames
+ // KF1 KF4 KF2 KF3 frames
// 1 1.5 2 3 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- f2->setTimeStamp(4);
+ F2->setTimeStamp(4);
// Trajectory status:
- // kf1 kf4 kf3 kf2 frames
+ // KF1 KF4 KF3 KF2 frames
// 1 1.5 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- f4->setTimeStamp(0.5);
+ F4->setTimeStamp(0.5);
// Trajectory status:
- // kf4 kf2 kf3 kf2 frames
+ // KF4 KF2 KF3 KF2 frames
// 0.5 1 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
+ ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
+
+ FrameBasePtr F5 = std::make_shared<FrameBase>(AUXILIARY, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ T->addFrame(F5);
+ // Trajectory status:
+ // KF4 KF2 AuxF5 KF3 KF2 frames
+ // 0.5 1 1.5 3 4 time stamps
+ // --+-----+-----+-----+-----+---> time
+ if (debug) P->print(2,0,1,0);
+ ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
+
+ F5->setTimeStamp(5);
+ // Trajectory status:
+ // KF4 KF2 KF3 KF2 AuxF5 frames
+ // 0.5 1 3 4 5 time stamps
+ // --+-----+-----+-----+-----+---> time
+ if (debug) P->print(2,0,1,0);
+ ASSERT_EQ(T->getLastFrame() ->id(), F5->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
+
+ FrameBasePtr F6 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ T->addFrame(F6);
+ // Trajectory status:
+ // KF4 KF2 KF3 KF2 AuxF5 F6 frames
+ // 0.5 1 3 4 5 6 time stamps
+ // --+-----+-----+-----+-----+-----+---> time
+ if (debug) P->print(2,0,1,0);
+ ASSERT_EQ(T->getLastFrame() ->id(), F6->id());
+ ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+ ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
}