diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index ac33da83c3fece5dce946e9c808869edd511e6f4..a705a480cef464c49c0ec6b2f463ecf527c825ec 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -141,7 +141,7 @@ int main()
TimeStamp t(0.0); // t : 0.0
Vector3d x(0,0,0);
Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+ problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
// SELF CALIBRATION ===================================================
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 6533bd414d54a9eb45da8c92336ba3d1f0e07739..8b399f668a8a4763c35186a309b1cfbf76f2a86b 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>("QUATERNION ABS",
nullptr,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index f2b19ef05c22f7a4c6777f889031dbda45d80a5b..8e5d7736eb0bd79c2e77948ae1e73d8a193f4d7f 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>
@@ -54,8 +63,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 !!
@@ -157,10 +166,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.
@@ -269,8 +292,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);
@@ -361,14 +382,9 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-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 81a9184ad4798ccbaac1ebd61a194f6e287331b4..e20da100d5f3dbd7f78df097eaa2634bae51b3e7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -134,6 +134,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
+ FIRST_TIME_WITHOUT_PACK, // NOT POSSIBLE
+ FIRST_TIME_WITH_PACK_BEFORE_INCOMING,
+ FIRST_TIME_WITH_PACK_ON_INCOMING,
+ FIRST_TIME_WITH_PACK_AFTER_INCOMING, // NOT POSSIBLE
RUNNING_WITHOUT_PACK,
RUNNING_WITH_PACK_BEFORE_ORIGIN,
RUNNING_WITH_PACK_ON_ORIGIN,
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 53acc960ecc87a61d28d7e1567af966d0579b375..189e0f99bc51a765893c8c52e55ec2ac0971ccf0 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -7,14 +7,20 @@
#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/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.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
@@ -36,8 +42,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)
@@ -59,7 +65,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()
@@ -153,21 +158,28 @@ ProblemPtr Problem::autoSetup(ParamsServer &_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()
{
@@ -233,10 +245,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;
}
@@ -277,11 +285,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;
}
@@ -868,51 +871,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;
+
+ 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;
+ }
+ }
+}
+
+FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
+{
+ assert(!isPriorSet() && "applyPriorOptions can be called once!");
+ FrameBasePtr prior_keyframe(nullptr);
- // emplace feature and factor
- init_capture->emplaceFeatureAndFactor();
+ if (prior_options_->mode != "nothing")
+ {
+ 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)
- WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
+ // Emplace a capture
+ auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
- // 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);
+ // 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)");
- prior_is_set_ = true;
+ // 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());
- // 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);
+ // 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 f61f861b021e51a08dd92db301aa1bd8de159272..8c3ba79053e98cc77e9c80c2cb6bebc64915cc91 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 aded1532cc17fc6a0f2e0d48edbbca857a1ab28b..5787c019254e612180a7ae3fca58cc5da1b6d2a8 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -93,6 +93,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
+ case FIRST_TIME_WITHOUT_PACK :
+ // TODO: create keyframe with zero state? asking problem emplaceFrame?
+ break;
+ case FIRST_TIME_WITH_PACK_BEFORE_INCOMING :
+ // TODO: create keyframe asking problem emplaceFrame
+ break;
+ case FIRST_TIME_WITH_PACK_ON_INCOMING :
+ // TODO: joint to this KF
+ break;
+ case FIRST_TIME_WITH_PACK_AFTER_INCOMING :
+ // TODO: create keyframe asking problem emplaceFrame
+ break;
case RUNNING_WITHOUT_PACK :
case RUNNING_WITH_PACK_ON_ORIGIN :
break;
@@ -588,40 +600,69 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
- if (!getProblem()->priorIsSet())
+// if (!getProblem()->priorIsSet())
+// {
+// 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.");
+// }
+
+ // 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(incoming_ptr_, params_motion_->time_tolerance);
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ if (pack)
+ {
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ processing_step_ = FIRST_TIME_WITH_PACK_ON_INCOMING;
+ else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ {
+ WOLF_INFO("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_PACK_BEFORE_INCOMING;
+ }
+ else
+ {
+ WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITH_PACK_AFTER_INCOMING;
+ }
+ }
+ else
+ {
+ WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITHOUT_PACK;
+ }
+ }
+ else
+ {
+ 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;
+ // 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, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ 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_PACK_ON_ORIGIN;
+ }
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+ processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
- else
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ else
+ processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ }
+ else
+ processing_step_ = RUNNING_WITHOUT_PACK;
}
- else
- processing_step_ = RUNNING_WITHOUT_PACK;
return pack;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 56d5ae4ae45ed40962274292328076087da7fb9f..59671c2308468052d364f3e12bff8aa4b828c625 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 1e06c7a80377b99197a721e85473319f9bf34862..cded07e0a3d882710f6b42e16a9460afb5e77611 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 67ef0974061dfa88339bbf3be362794ec6af060d..2f6ebb53e7f315ebd82db9f172f8b43d00d8470a 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -449,7 +449,7 @@ 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);
+ auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -581,7 +581,7 @@ 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);
+ auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 1176783c855954afba8bf5e0a7566fb37ecbc8b5..0929163a71e7ea0e1a2d31ca1958b39e7a2cb902 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;
@@ -217,7 +217,7 @@ TEST(Odom2d, VoteForKfAndSolve)
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
+ FrameBasePtr origin_frame = problem->setPriorFactor(x0, P0, t0, dt/2);
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -352,7 +352,7 @@ TEST(Odom2d, KF_callback)
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
// Check covariance values
Eigen::Vector3d integrated_pose = x0;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 6d262ba06b81438499b29ab3d45fd4bb101c89db..f9800e3f3ee2495ad445de725d284d4ef943ceed 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -141,7 +141,7 @@ TEST(Problem, SetOrigin_PO_2d)
Eigen::VectorXd x0(3); x0 << 1,2,3;
Eigen::MatrixXd P0(3,3); 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);
@@ -151,18 +151,29 @@ 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);
+ 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 one frame, one capture, one feature, one factor
+ 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_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
@@ -178,7 +189,7 @@ TEST(Problem, SetOrigin_PO_3d)
Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7;
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);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index c4a1c4b0470c92bf09df1a950a1c19e57d35642d..96db133bba8e3efa62e6dd68fc0c1331d24623fd 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -104,7 +104,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));
@@ -117,15 +117,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 612f66019cd62b892a300df15da4a809b0d51e9a..d68ace3de5e37428d21d829ef3632008983a436f 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -311,7 +311,7 @@ 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);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
@@ -341,7 +341,7 @@ 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);
// Straight one turn of the wheels, in one go
data(0) = 100.0 ; // one turn of the wheels
@@ -366,7 +366,7 @@ 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);
// 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 d00fb93407ab75bb150a576bc434640f1c75fe12..5fb84268d1c376955e8cb2c7a6ed4edbb7536c11 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 076118ba3d7836dbf61cdb2ed1d38d81d46b9084..6170c83c7efbc2f6bbe91b71b289874294ddce40 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -75,7 +75,7 @@ class ProcessorMotion_test : public testing::Test{
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPrior(x0, P0, 0.0, 0.01);
+ problem->setPriorFactor(x0, P0, 0.0, 0.01);
}
virtual void TearDown(){}