From ac86bd1e1327fbbb64aaa989a6c7958edbc884ff Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 22 Feb 2018 17:20:18 +0100
Subject: [PATCH] Make priorIsSet()
---
src/problem.cpp | 10 +++++-----
src/problem.h | 6 +++++-
2 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/src/problem.cpp b/src/problem.cpp
index 8b80490b1..2af2f2148 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -31,7 +31,7 @@ Problem::Problem(const std::string& _frame_structure) :
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_motion_ptr_(),
- origin_is_set_(false),
+ prior_is_set_(false),
state_size_(0),
state_cov_size_(0)
{
@@ -121,7 +121,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
_corresponding_sensor_ptr->addProcessor(prc_ptr);
// setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && origin_is_set_)
+ if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFramePtr());
// setting the main processor motion
@@ -334,7 +334,7 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
- processor->keyFrameCallbackNew(_keyframe_ptr, _time_tolerance);
+ processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
}
LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
@@ -630,7 +630,7 @@ StateBlockList& Problem::getStateBlockList()
FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
- if (!origin_is_set_)
+ if ( ! prior_is_set_ )
{
// Create origin frame
FrameBasePtr origin_frame_ptr = emplaceFrame(KEY_FRAME, _prior_state, _ts);
@@ -654,7 +654,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
if (processor_ptr->isMotion())
(std::static_pointer_cast<ProcessorMotion>(processor_ptr))->setOrigin(origin_frame_ptr);
- origin_is_set_ = true;
+ prior_is_set_ = true;
keyFrameCallback(origin_frame_ptr, nullptr, _time_tolerance);
diff --git a/src/problem.h b/src/problem.h
index e12589757..adaa9c823 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -54,7 +54,7 @@ class Problem : public std::enable_shared_from_this<Problem>
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
std::list<StateBlockNotification> state_block_notification_list_;
std::list<ConstraintNotification> constraint_notification_list_;
- bool origin_is_set_;
+ bool prior_is_set_;
Size state_size_, state_cov_size_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -312,6 +312,10 @@ class Problem : public std::enable_shared_from_this<Problem>
bool state_blocks = false);
bool check(int verbose_level = 0);
+ bool priorIsSet() const
+ {
+ return prior_is_set_;
+ }
};
} // namespace wolf
--
GitLab