diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 0743548b7b4aad0517ecdd2590955c4cdf7c023c..4ff0a96e89ee4bb9b6b5a77534e6656da24d7269 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -31,15 +31,43 @@ class FactorBlockAbsolute : public FactorAnalytic
/** \brief Constructor
*
* \param _sb_ptr the constrained state block
- * \param _start_idx (default=0) the index of the first state element that is constrained
- * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
*
*/
FactorBlockAbsolute(StateBlockPtr _sb_ptr,
- unsigned int _start_idx = 0,
- int _size = -1,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorBlockAbsolute",
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
+ sb_size_(_sb_ptr->getSize()),
+ sb_constrained_start_(0),
+ sb_constrained_size_(sb_size_)
+ {
+ assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+
+ // precompute Jacobian (Identity)
+ J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
+ }
+
+ /** \brief Constructor for segment of state block
+ *
+ * \param _sb_ptr the constrained state block
+ * \param _start_idx the index of the first state element that is constrained
+ * \param _start_idx the size of the state segment that is constrained, -1 = all the
+ *
+ */
+ FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ unsigned int _start_idx,
+ int _size,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
nullptr,
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 402615a5e5c4f11d1f306d57119cb7668031c459..aca1f7f966aa33e3f970e630b984b7fe145781ed 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -24,8 +24,8 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
public:
FactorQuaternionAbsolute(StateBlockPtr _sb_ptr,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
nullptr,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 3da679fe15b63d7a11c011e85855ea06ef564fdf..7dc2caca9fdbe5fc8527cab6814a03665b0ac89e 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -34,6 +34,15 @@ enum Notification
REMOVE
};
+struct PriorOptions
+{
+ std::string mode = "";
+ Eigen::VectorXd state;
+ Eigen::MatrixXd cov;
+ double time_tolerance;
+};
+WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
+
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
@@ -55,8 +64,8 @@ class Problem : public std::enable_shared_from_this<Problem>
mutable std::mutex mut_factor_notifications_;
mutable std::mutex mut_state_block_notifications_;
mutable std::mutex mut_covariances_;
- bool prior_is_set_;
std::string frame_structure_;
+ PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
@@ -162,10 +171,24 @@ class Problem : public std::enable_shared_from_this<Problem>
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
- virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
- const Eigen::MatrixXd& _prior_cov, //
- const TimeStamp& _ts,
- const double _time_tolerance);
+
+ // Prior
+ bool isPriorSet() const;
+ void setPriorOptions(const std::string& _mode,
+ const double _time_tolerance = 0,
+ const Eigen::VectorXd& _state = Eigen::VectorXd(0),
+ const Eigen::MatrixXd& _cov = Eigen::MatrixXd(0,0));
+ FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
+ FrameBasePtr setPriorFactor(const Eigen::VectorXd &_state,
+ const Eigen::MatrixXd &_cov,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorFix(const Eigen::VectorXd &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorInitialGuess(const Eigen::VectorXd &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
/** \brief Emplace frame from string frame_structure
* \param _frame_structure String indicating the frame structure.
@@ -274,8 +297,6 @@ class Problem : public std::enable_shared_from_this<Problem>
void getState (const TimeStamp& _ts, Eigen::VectorXd& _state) const;
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
- bool priorIsSet() const;
- void setPriorIsSet(bool _prior_is_set);
// Perturb state
void perturb(double amplitude = 0.01);
@@ -371,14 +392,9 @@ inline TreeManagerBasePtr Problem::getTreeManager() const
return tree_manager_;
}
-inline bool Problem::priorIsSet() const
-{
- return prior_is_set_;
-}
-
-inline void Problem::setPriorIsSet(bool _prior_is_set)
+inline bool Problem::isPriorSet() const
{
- prior_is_set_ = _prior_is_set;
+ return prior_options_ == nullptr;
}
inline IsMotionPtr Problem::getProcessorIsMotion()
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 36ebafaea99994c11f9bea31781a159628073eac..63d55436c2c2f0ccd02f8a33cbbc378900718922 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -134,10 +134,14 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
- RUNNING_WITHOUT_PACK,
- RUNNING_WITH_PACK_BEFORE_ORIGIN,
- RUNNING_WITH_PACK_ON_ORIGIN,
- RUNNING_WITH_PACK_AFTER_ORIGIN
+ FIRST_TIME_WITHOUT_KF,
+ FIRST_TIME_WITH_KF_BEFORE_INCOMING,
+ FIRST_TIME_WITH_KF_ON_INCOMING,
+ FIRST_TIME_WITH_KF_AFTER_INCOMING,
+ RUNNING_WITHOUT_KF,
+ RUNNING_WITH_KF_BEFORE_ORIGIN,
+ RUNNING_WITH_KF_ON_ORIGIN,
+ RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
protected:
@@ -211,11 +215,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
-
+ // Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
- // Helper functions:
protected:
/** \brief process an incoming capture
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6545a6d346c22bb24de7696c45e5d78c7481fbe5..7acce5acb13de2ec58e52f76197932119e7a86a1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -7,16 +7,22 @@
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
+#include "core/capture/capture_void.h"
#include "core/factor/factor_base.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/factor/factor_quaternion_absolute.h"
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_base.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.h"
#include "core/utils/loader.h"
#include "core/utils/check_log.h"
+#include "core/math/covariance.h"
// IRI libs includes
@@ -39,8 +45,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_is_motion_list_(std::list<IsMotionPtr>()),
- prior_is_set_(false),
- frame_structure_(_frame_structure)
+ frame_structure_(_frame_structure),
+ prior_options_(std::make_shared<PriorOptions>())
{
dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
@@ -62,7 +68,6 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
-
}
void Problem::setup()
@@ -169,21 +174,28 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
problem->setTreeManager(AutoConfFactoryTreeManager::get().create(tree_manager_type, "tree manager", _server));
// Prior
- Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
- Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
- double prior_time_tolerance = _server.getParam<double>("problem/prior/time_tolerance");
- double prior_ts = _server.getParam<double>("problem/prior/timestamp");
-
- WOLF_TRACE("prior timestamp:\n" , prior_ts);
- WOLF_TRACE("prior state:\n" , prior_state.transpose());
- WOLF_TRACE("prior covariance:\n" , prior_cov);
- WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);
-
- problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);
+ std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
+ if (prior_mode == "nothing")
+ {
+ problem->setPriorOptions(prior_mode);
+ }
+ else if (prior_mode == "factor")
+ {
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/time_tolerance"),
+ _server.getParam<Eigen::VectorXd>("problem/prior/state"),
+ _server.getParam<Eigen::MatrixXd>("problem/prior/cov"));
+ }
+ else
+ {
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/timestamp"),
+ _server.getParam<Eigen::VectorXd>("problem/prior/state"));
+ }
// Done
return problem;
- }
+}
Problem::~Problem()
{
@@ -249,10 +261,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
return prc_ptr;
}
@@ -293,11 +301,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
-
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
return prc_ptr;
}
@@ -405,7 +408,9 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
// an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
- for (auto proc: processor_is_motion_list_){
+
+ for (auto proc: processor_is_motion_list_)
+ {
Eigen::VectorXd proc_state = proc->getState(_ts);
int idx = 0;
@@ -516,6 +521,7 @@ Eigen::VectorXd Problem::zeroState() const
// Set the quaternion identity for 3d states. Set only the real part to 1:
if(this->getDim() == 3)
state(6) = 1.0;
+
return state;
}
@@ -898,55 +904,133 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons
return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}
-FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen::MatrixXd& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
+void Problem::setPriorOptions(const std::string& _mode,
+ const double _time_tolerance,
+ const Eigen::VectorXd& _state,
+ const Eigen::MatrixXd& _cov)
{
- if ( ! prior_is_set_ )
+ assert(prior_options_ != nullptr && "prior options have already been applied");
+ assert(prior_options_->mode == "" && "prior options have already been set");
+ assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
+
+ // Store options (optionals depending on the mode)
+ WOLF_TRACE("prior mode: ", _mode);
+ prior_options_->mode = _mode;
+
+ if (prior_options_->mode != "nothing")
{
- // Create origin frame
- 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
- CapturePosePtr init_capture;
- // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
- // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- // else
- // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
-
- if (this->getDim() == 3)
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- else
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+ assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+ WOLF_TRACE("prior state: ", _state.transpose());
+ WOLF_TRACE("prior time tolerance: ", _time_tolerance);
+ prior_options_->state = _state;
+ prior_options_->time_tolerance = _time_tolerance;
- // emplace feature and factor
- init_capture->emplaceFeatureAndFactor();
+ if (prior_options_->mode == "factor")
+ {
+ assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)");
+ WOLF_TRACE("prior covariance:\n" , _cov);
+ prior_options_->cov = _cov;
+ }
+ }
+}
- WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
+FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
+{
+ assert(!isPriorSet() && "applyPriorOptions can be called once!");
- // Notify all motion processors about the prior KF
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if (processor->isMotion())
- // Motion processors will set its origin at the KF
- (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
+ FrameBasePtr prior_keyframe(nullptr);
- prior_is_set_ = true;
+ if (prior_options_->mode != "nothing" and prior_options_->mode != "")
+ {
+ prior_keyframe = emplaceFrame(this->getFrameStructure(),
+ this->getDim(),
+ KEY,
+ prior_options_->state,
+ _ts);
+
+ if (prior_options_->mode == "fix")
+ prior_keyframe->fix();
+ else if (prior_options_->mode == "factor")
+ {
+ // FIXME: change whenever state is changed to a map<string,vector>
+ // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
- // Notify all other processors about the origin KF --> they will join it or not depending on their received data
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if ( !processor->isMotion() )
- processor->keyFrameCallback(origin_keyframe, _time_tolerance);
+ // Emplace a capture
+ auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
+
+ // Emplace a feature and a factor for each state block
+ int state_idx = 0;
+ int cov_idx = 0;
+ for (auto sb : prior_keyframe->getStateBlockVec())
+ {
+ assert(sb != nullptr);
+ assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
+ assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
- // Notify tree manager
- if (tree_manager_)
- tree_manager_->keyFrameCallback(origin_keyframe);
+ // state block segment
+ Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
+ Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+
+ // feature
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+
+ // factor
+ if (sb->hasLocalParametrization())
+ {
+ if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
+ else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ else
+ throw std::runtime_error("not implemented...!");
+ }
+ else
+ {
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ }
+ state_idx += sb->getSize();
+ cov_idx += sb->getLocalSize();
+ }
+ assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
+ assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
+ }
+ else
+ assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
- return origin_keyframe;
+ // notify all processors
+ keyFrameCallback(prior_keyframe, nullptr, prior_options_->time_tolerance);
}
- else
- throw std::runtime_error("Origin already set!");
+ // remove prior options
+ prior_options_ = nullptr;
+
+ return prior_keyframe;
+}
+
+FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state,
+ const Eigen::MatrixXd &_cov,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("factor", _time_tol, _state, _cov);
+ return applyPriorOptions(_ts);
+}
+
+
+FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("fix", _time_tol, _state);
+ return applyPriorOptions(_ts);
+}
+
+FrameBasePtr Problem::setPriorInitialGuess(const Eigen::VectorXd &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("initial_guess", _time_tol, _state);
+ return applyPriorOptions(_ts);
}
void Problem::loadMap(const std::string& _filename_dot_yaml)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 0f1bf312f9b6ac5ca5aecf80b954a00271569352..364dc699740595f3f36a8af86e4d8ea290af585c 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -53,6 +53,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ // apply prior in problem if not done (very first capture)
+ if (getProblem() && !getProblem()->isPriorSet())
+ getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+
// asking if capture should be stored
if (storeCapture(_capture_ptr))
buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index b2bf22587e0fdcb7db821aaa14eca842357b1a8c..bc11469c5c8aa5a5bb4c26c5a41f632c3f89312f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -24,7 +24,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
ParamsProcessorMotionPtr _params_motion) :
ProcessorBase(_type, _dim, _params_motion),
params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_PACK),
+ processing_step_(RUNNING_WITHOUT_KF),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -101,11 +101,35 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
- case RUNNING_WITHOUT_PACK :
- case RUNNING_WITH_PACK_ON_ORIGIN :
+ case FIRST_TIME_WITHOUT_KF :
+ {
+ // There is no KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_ON_INCOMING :
+ {
+ // can joint to the KF
+ setOrigin(pack->key_frame);
+ break;
+ }
+ case FIRST_TIME_WITH_KF_AFTER_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case RUNNING_WITHOUT_KF :
+ case RUNNING_WITH_KF_ON_ORIGIN :
break;
- case RUNNING_WITH_PACK_BEFORE_ORIGIN :
+ case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
/*
@@ -192,7 +216,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case RUNNING_WITH_PACK_AFTER_ORIGIN :
+ case RUNNING_WITH_KF_AFTER_ORIGIN :
{
/*
* Legend:
@@ -326,8 +350,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new frame
auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getProblem()->getCurrentState(),
- getCurrentTimeStamp());
+ getProblem()->getCurrentState(),
+ getCurrentTimeStamp());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -362,11 +386,15 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
CaptureMotionPtr capture_motion;
if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
+ {
// timestamp found in the current processor buffer
capture_motion = last_ptr_;
+ }
else
+ {
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
capture_motion = findCaptureContainingTimeStamp(_ts);
+ }
if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
@@ -414,11 +442,13 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
+
// -------- ORIGIN ---------
// emplace (empty) origin Capture
origin_ptr_ = emplaceCapture(_origin_frame,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -427,15 +457,14 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
+ _origin_frame->getState(),
origin_ts);
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -596,42 +625,70 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
- if (!getProblem()->priorIsSet())
+ // Origin not set
+ if (!origin_ptr_)
{
- WOLF_WARN ("||*||");
- WOLF_INFO (" ... It seems you missed something!");
- WOLF_ERROR("ProcessorMotion received data before being initialized.");
- WOLF_INFO ("Did you forget to issue a Problem::setPrior()?");
- throw std::runtime_error("ProcessorMotion received data before being initialized.");
- }
-
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(incoming_ptr_, params_motion_->time_tolerance);
- // ignore "future" KF to avoid MotionBuffer::split() error
- if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
- pack = nullptr;
+ if (pack)
+ {
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ {
+ WOLF_DEBUG("First time with a KF compatible.")
+ processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
+ }
+ else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ {
+ WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
+ }
+ else
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
+ }
+ }
+ else
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITHOUT_KF;
+ }
- if (pack)
+ return pack;
+ }
+ else
{
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+
+ // ignore "future" KF to avoid MotionBuffer::split() error
+ if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
+ pack = nullptr;
+
+ if (pack)
{
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
- }
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
- processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ {
+ WOLF_WARN("||*||");
+ WOLF_INFO(" ... It seems you missed something!");
+ WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+ // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+ processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
+ }
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+ processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
+
+ else
+ processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
+ }
else
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ processing_step_ = RUNNING_WITHOUT_KF;
+ return pack;
}
- else
- processing_step_ = RUNNING_WITHOUT_PACK;
- return pack;
+ // not reached
+ return nullptr;
}
void ProcessorMotion::setProblem(ProblemPtr _problem)
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 6119b6c487b56f2a7c48af70131e0570fa54d1dd..9750daa633b6a12a95ddf8e4034a792b816104f3 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -297,7 +297,6 @@ void ProcessorTracker::computeProcessingStep()
WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
- WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed ---> ", (getProblem()->priorIsSet() ? "OK" : "NOK"));
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 09afa700e79e66b058df609ea004334bd0830e4b..07cd3697f6a7b3e1ae085b3a51cc158298e982b6 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -184,9 +184,9 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX
// create & add factor absolute
if (is_quaternion)
- FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb);
+ FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb, nullptr, false);
else
- FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size);
+ FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size, nullptr, false);
// store feature in params_prior_map_
params_prior_map_[_key] = ftr_prior;
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index c0f75e3e8753a568fafb772805910642031494d4..14dea0d458afbb8fa6223f0bfc8d286bc2a473e6 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -49,7 +49,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -67,7 +67,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -83,7 +83,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -101,7 +101,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
+ FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index f661fb37fd65f4f99ecd7015d4e8474688521a18..594eb169754b47fb3304536742ea445051e4da85 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -50,11 +50,11 @@ class FixtureFactorBlockDifference : public testing::Test
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+ //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
+ //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
+ //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index b10279093851116c81d1b1bd8c608e79fa8cbf25..fd82dd83817fc2fe6991f0e8ba6bea09bf8c431e 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -441,7 +441,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
Vector3d x0(0,0,0);
@@ -449,7 +448,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- auto F0 = problem->setPrior(x0, P0, t, 0.1);
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -459,7 +460,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -481,14 +482,13 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto F2 = problem->getLastKeyFrame();
-
// Fix boundaries
F0->fix();
F2->fix();
// Perturb S
- Vector3d calib_pert = calib_gt + Vector3d::Random()*0.2;
- sensor->getIntrinsic()->setState(calib_pert);
+ sensor->getIntrinsic()->perturb(0.2);
+ Vector3d calib_pert = sensor->getIntrinsic()->getState();
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -573,7 +573,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
Vector3d x0(0,0,0);
@@ -581,7 +580,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- auto F0 = problem->setPrior(x0, P0, t, 0.1);
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -591,7 +592,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -621,7 +622,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
F0->fix();
F2->fix();
-
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 0c61b6d75634426e3b1a487bd6339c30be270a55..33026ee4b5f72b7ce06cdf79c3c765963e0cb64c 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -128,7 +128,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2);
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, P0,t0, dt/2);
// KF1 and motion from KF0
t += dt;
@@ -200,9 +200,10 @@ TEST(Odom2d, VoteForKfAndSolve)
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
+ params->max_time_span = 100;
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.00;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -211,13 +212,13 @@ TEST(Odom2d, VoteForKfAndSolve)
// NOTE: We integrate and create KFs as follows:
// i= 0 1 2 3 4 5 6
- // KF -- * -- * -- KF - * -- * -- KF - *
+ // KF - * -- * -- KF - * -- * -- KF - *
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame
- FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
+ // Origin Key Frame (in t1 to let processor motion to join the KF)
+ problem->setPriorFactor(x0, P0, t+dt, dt/2);
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -246,7 +247,7 @@ TEST(Odom2d, VoteForKfAndSolve)
for (int n=1; n<=N; n++)
{
- // std::cout << "-------------------\nStep " << i << " at time " << t << std::endl;
+ std::cout << "-------------------\nStep " << n << " at time " << t << std::endl;
// re-use capture with updated timestamp
capture->setTimeStamp(t);
@@ -269,9 +270,9 @@ TEST(Odom2d, VoteForKfAndSolve)
integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
}
- WOLF_DEBUG("n: ", n, " t:", t);
- WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
- WOLF_DEBUG("test delta: ", integrated_delta .transpose());
+ WOLF_INFO("n: ", n, " t:", t);
+ WOLF_INFO("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
+ WOLF_INFO("test delta: ", integrated_delta .transpose());
ASSERT_POSE2d_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
@@ -339,9 +340,10 @@ TEST(Odom2d, KF_callback)
SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->dist_traveled = 100;
- params->angle_turned = 6.28;
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
params->max_time_span = 100;
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.000001;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -351,8 +353,9 @@ TEST(Odom2d, KF_callback)
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
+ // Origin Key Frame (in t1 to let processor motion to join the KF)
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0+dt, dt/2);
+ processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
Eigen::Vector3d integrated_pose = x0;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 5049df30524900e9a998f961f55d9d9efc5d4275..bf878e87c1cb5a277225ac20be11fce7f1b12c5d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -136,9 +136,11 @@ TEST(Problem, SetOrigin_PO_2d)
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
Eigen::VectorXd x0(3); x0 << 1,2,3;
- Eigen::MatrixXd P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
- P->setPrior(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, P0, t0, 1.0);
+
+ P->print(4,1,1,1);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
@@ -148,22 +150,36 @@ TEST(Problem, SetOrigin_PO_2d)
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
+ FeatureBasePtr fp = C->getFeatureList().front();
+ FeatureBasePtr fo = C->getFeatureList().back();
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ // check that we have one capture (prior)
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
+ // check that we have two features (prior: one per state block)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -172,10 +188,10 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7;
+ Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
- P->setPrior(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, P0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
@@ -185,22 +201,36 @@ TEST(Problem, SetOrigin_PO_3d)
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
+ FeatureBasePtr fp = C->getFeatureList().front();
+ FeatureBasePtr fo = C->getFeatureList().back();
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ // check that we have one capture (prior)
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
+ // check that we have two features (prior: one per state block)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
+
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
// 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.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 22b957b1b2026bf91546bd7a3a863a37b8037266..4ac7acf50f3fc066901858db2b34843c58634af3 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -98,7 +98,7 @@ TEST(ProcessorBase, KeyFrameCallback)
TimeStamp t(0.0);
Vector3d x(0,0,0);
Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
+ problem->setPriorFactor(x, P, t, dt/2); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
@@ -111,15 +111,21 @@ TEST(ProcessorBase, KeyFrameCallback)
// Move
t = t+dt;
WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ std::cout << "1\n";
capt_odo->setTimeStamp(t);
+ std::cout << "2\n";
proc_odo->captureCallback(capt_odo);
+ std::cout << "3\n";
// Track
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
+ std::cout << "4\n";
proc_trk->captureCallback(capt_trk);
+ std::cout << "5\n";
- problem->print(4,1,1,0);
+ problem->print(4,1,1,0);
+ std::cout << "6\n";
// Only odom creating KFs
ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index c88628d779ef9b5836c3d5e2e7a061012c23d0a8..7704ec8c4e720ae0060f22dfdccaed512383692e 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -311,7 +311,8 @@ TEST_F(ProcessorDiffDriveTest, process)
Vector3d x(0,0,0);
Matrix3d P; P.setIdentity();
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
@@ -320,14 +321,12 @@ TEST_F(ProcessorDiffDriveTest, process)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
for (int n = 1; n <= N; n++)
{
- C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
-
t += dt;
C->setTimeStamp(t);
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
}
problem->print(4,1,1,1);
@@ -341,7 +340,8 @@ TEST_F(ProcessorDiffDriveTest, linear)
Vector3d x(0,0,0);
Matrix3d P; P.setIdentity();
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = 100.0 ; // one turn of the wheels
@@ -366,7 +366,8 @@ TEST_F(ProcessorDiffDriveTest, angular)
Vector3d x(0,0,0);
Matrix3d P; P.setIdentity();
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 6b141dfb11370611da1c60c7be539c0deb8d8c47..5d5bb796004cb66a817a5fb546eee22da35e0ba5 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -79,7 +79,7 @@ TEST(ProcessorLoopClosure, installProcessor)
TimeStamp t(0.0);
Vector3d x(0,0,0);
Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
+ problem->setPriorFactor(x, P, t, dt/2); // KF1
// new KF
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 19febcc6fa8b869b3685b59ca7aeba1e2d6ecf31..8f4738dae79311399a88bf15487c729741eba888 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -52,7 +52,7 @@ class ProcessorMotion_test : public testing::Test{
Matrix2d data_cov;
// ProcessorMotion_test() :
-// ProcessorOdom2d(std::make_shared<ParamsProcessorOdom2d>()),
+// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
// dt(0)
// { }
@@ -72,21 +72,72 @@ class ProcessorMotion_test : public testing::Test{
params->unmeasured_perturbation_std = 0.001;
processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, 3, 3, nullptr);
-
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- problem->setPrior(x0, P0, 0.0, 0.01);
}
virtual void TearDown(){}
};
-TEST_F(ProcessorMotion_test, IntegrateStraight)
+TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
+ // Prior
+ TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
+
data << 1, 0; // advance straight
data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
for (int i = 0; i<9; i++)
{
@@ -101,8 +152,12 @@ TEST_F(ProcessorMotion_test, IntegrateStraight)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateCircle)
+TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -120,11 +175,155 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, splitBuffer)
+TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
+ // Prior
+ TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ 3,
+ 3,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ 3,
+ 3,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFixPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
for (int i = 0; i<10; i++) // one full turn exactly
{
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index f7579f2cd0dd93b403b363afcfe9d4bc8e5aad77..604cd09b4f20ed68e1c44273f85a93c8c1a88c1a 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -283,7 +283,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
{
//1ST TIME -> KF (origin)
- WOLF_DEBUG("First time...");
+ WOLF_INFO("First time...");
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
@@ -292,7 +292,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//2ND TIME
- WOLF_DEBUG("Second time...");
+ WOLF_INFO("Second time...");
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process();
@@ -301,7 +301,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//3RD TIME
- WOLF_DEBUG("Third time...");
+ WOLF_INFO("Third time...");
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process();
@@ -309,7 +309,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//4TH TIME
- WOLF_DEBUG("Forth time...");
+ WOLF_INFO("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process();
@@ -317,7 +317,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
- WOLF_DEBUG("Fifth time...");
+ WOLF_INFO("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
@@ -326,7 +326,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
// check factors
- WOLF_DEBUG("checking factors...");
+ WOLF_INFO("checking factors...");
TrackMatrix track_matrix = processor->getTrackMatrix();
ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
ASSERT_TRUE(problem->check(0));
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 5799117ffa523a9cbf5132ed504fa1525001c0ae..4f609da767afb85bbc6cc818906524612aff3ded 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -64,6 +64,7 @@ TEST(TreeManager, autoConf)
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
@@ -76,6 +77,7 @@ TEST(TreeManager, autoConfNone)
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index ed3a8052bfb4a73b4395cc885904d8f752af91d2..4cf8cf524ef114f357919661a3aee62a65e34234 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -76,6 +76,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
// FRAME 1 ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastKeyFrame();
@@ -171,8 +172,9 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
- // FRAME 1 ----------------------------------------------------------
+ // FRAME 1 (prior) ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastKeyFrame();
ASSERT_TRUE(F1 != nullptr);
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index 25e8ac8a4417ffec910fee4cac96669a00ebdc4f..085015b89c62f913f7bbf6ab3d6241ddde132dff 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -3,10 +3,10 @@ config:
frame_structure: "POV"
dimension: 3
prior:
+ mode: "factor"
state: [0,0,0,0,0,0,1,0,0,0]
cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
time_tolerance: 0.1
- timestamp: 0
tree_manager:
type: "TreeManagerDummy"
toy_param: 0
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index 913f5875bfd09750e024fcfeab910ff45d6058ee..80b5af47ac509b0746ffb8f66775d87742e1e927 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -3,10 +3,10 @@ config:
frame_structure: "POV"
dimension: 3
prior:
+ mode: "factor"
state: [0,0,0,0,0,0,1,0,0,0]
cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
time_tolerance: 0.1
- timestamp: 0
tree_manager:
type: "None"
sensors:
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 5cbb9e5a787b4ad2e70f9cf7f283ab1747e1279b..3a0f421c5c0048125d0a8afa742e873f79e703f6 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -3,10 +3,10 @@ config:
frame_structure: "PO"
dimension: 3
prior:
+ mode: "factor"
state: [0,0,0,0,0,0,1]
cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
time_tolerance: 0.1
- timestamp: 0
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: 3
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 01f41eea97c7cca943d4aa12a143736668c2673f..894dadf651dfe18919881ca44a296f3df447246a 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -3,10 +3,10 @@ config:
frame_structure: "PO"
dimension: 3
prior:
+ mode: "factor"
state: [0,0,0,0,0,0,1]
cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
time_tolerance: 0.1
- timestamp: 0
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: 3