diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 99e6d44b4271a7975e2570a3dc889d2c4d89989b..7bbac43e6aadc040886f8d4ffdcb9b5a023c3e19 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -310,6 +310,7 @@ SET(SRCS_BASE processor_tracker_feature.cpp processor_tracker_landmark.cpp sensor_base.cpp + state_block.cpp track_matrix.cpp time_stamp.cpp trajectory_base.cpp diff --git a/src/capture_base.cpp b/src/capture_base.cpp index 71a94387deaf352d9f1230de56513de7a470e1dc..e860b5757b882d5382ba0752b9dbe87d42aa89bd 100644 --- a/src/capture_base.cpp +++ b/src/capture_base.cpp @@ -182,11 +182,7 @@ void CaptureBase::fix() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -197,11 +193,7 @@ void CaptureBase::unfix() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -212,11 +204,7 @@ void CaptureBase::fixExtrinsics() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -227,11 +215,7 @@ void CaptureBase::unfixExtrinsics() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -242,11 +226,7 @@ void CaptureBase::fixIntrinsics() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -257,11 +237,7 @@ void CaptureBase::unfixIntrinsics() { auto sbp = getStateBlockPtr(i); if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } diff --git a/src/capture_base.h b/src/capture_base.h index 1d01e42841c91fa84b86ef7d1b21ccbc9a784ff8..3c14dc5e1cd96908f91e45d81763e2191eae5bf1 100644 --- a/src/capture_base.h +++ b/src/capture_base.h @@ -59,6 +59,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void setFramePtr(const FrameBasePtr _frm_ptr); void unlinkFromFrame(){frame_ptr_.reset();} + virtual void setProblem(ProblemPtr _problem) final; + FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr); FeatureBaseList& getFeatureList(); void addFeatureList(FeatureBaseList& _new_ft_list); @@ -109,6 +111,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture #include "sensor_base.h" #include "frame_base.h" #include "feature_base.h" +#include "state_block.h" namespace wolf{ @@ -117,6 +120,15 @@ inline SizeEigen CaptureBase::getCalibSize() const return calib_size_; } +inline void CaptureBase::setProblem(ProblemPtr _problem) +{ + NodeBase::setProblem(_problem); + for (auto ft : feature_list_) + ft->setProblem(_problem); + for (auto sb : state_block_vec_) + if (sb) sb->setProblem(_problem); +} + inline void CaptureBase::updateCalibSize() { calib_size_ = computeCalibSize(); diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index e11b7369d8eb3176d992ddbe71ef1403838486ff..36caa290b20d8be1bbe741dab144590c61791031 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/src/ceres_wrapper/qr_manager.cpp @@ -42,7 +42,7 @@ std::string QRManager::solve(const unsigned int& _report_level) // update state blocks for (auto sb_pair : sb_2_col_) - sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize())); + sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false); if (_report_level == 1) return std::string("Success!\n"); diff --git a/src/feature_base.cpp b/src/feature_base.cpp index 2a027b1a0ef384b2e30da68b84ea16eac62b03cc..d57a9a88f2a5c74a1c38e81fc9a16a59b11d6f3b 100644 --- a/src/feature_base.cpp +++ b/src/feature_base.cpp @@ -112,6 +112,13 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info) measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info); } +void FeatureBase::setProblem(ProblemPtr _problem) +{ + NodeBase::setProblem(_problem); + for (auto ctr : constraint_list_) + ctr->setProblem(_problem); +} + Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const { // impose symmetry diff --git a/src/feature_base.h b/src/feature_base.h index 01cd2fc2e65c7d5f495e0eccb20cf82f17acdbbf..3bd60033a40929c7e6a18705acbd46a2a56a94e3 100644 --- a/src/feature_base.h +++ b/src/feature_base.h @@ -48,6 +48,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature virtual ~FeatureBase(); void remove(); + virtual void setProblem(ProblemPtr _problem) final; + // properties unsigned int id(); unsigned int trackId(){return track_id_;} diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 123c4aac5ada62dd2377924ea136f0e45ead62bd..088e9effe597908d556b1e8a8f8ea48226fe4822 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -139,22 +139,14 @@ void FrameBase::fix() { for (auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } void FrameBase::unfix() { for (auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } bool FrameBase::isFixed() const @@ -186,7 +178,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state) for (StateBlockPtr sb : state_block_vec_) if (sb && (index < _st_size)) { - sb->setState(_state.segment(index, sb->getSize())); + sb->setState(_state.segment(index, sb->getSize()), isKey()); index += sb->getSize(); } } diff --git a/src/frame_base.h b/src/frame_base.h index 758f2eef7f05b0015d6e5dd0eb80dc2a6447923d..8f49e9b70a9275b590466fe5edcc728cb731605b 100644 --- a/src/frame_base.h +++ b/src/frame_base.h @@ -122,6 +122,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase // Wolf tree access --------------------------------------------------- public: + virtual void setProblem(ProblemPtr _problem) final; + TrajectoryBasePtr getTrajectoryPtr() const; void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr); @@ -258,6 +260,15 @@ inline unsigned int FrameBase::getHits() const return constrained_by_list_.size(); } +inline void FrameBase::setProblem(ProblemPtr _problem) +{ + NodeBase::setProblem(_problem); + for (auto cap : capture_list_) + cap->setProblem(_problem); + for (auto sb : state_block_vec_) + if (sb) sb->setProblem(_problem); +} + inline ConstraintBaseList& FrameBase::getConstrainedByList() { return constrained_by_list_; diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp index 8ff4ae919211cf0ec4bc7328f44bd9abcc3b1fe9..b84c078162635f2320abe46abd6e4c8a3c93cc61 100644 --- a/src/landmark_base.cpp +++ b/src/landmark_base.cpp @@ -18,6 +18,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State { state_block_vec_[0] = _p_ptr; state_block_vec_[1] = _o_ptr; + // std::cout << "constructed +L" << id() << std::endl; } @@ -54,22 +55,14 @@ void LandmarkBase::fix() { for (auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } void LandmarkBase::unfix() { for (auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } bool LandmarkBase::isFixed() const diff --git a/src/landmark_base.h b/src/landmark_base.h index 50a5608f97adcb35208f0dd82e79848a8c3bfe91..5bc32c90083aacb06edf3ce160c54bdc90334c76 100644 --- a/src/landmark_base.h +++ b/src/landmark_base.h @@ -88,6 +88,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma // Navigate wolf tree + virtual void setProblem(ProblemPtr _problem) final; + ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr); unsigned int getHits() const; ConstraintBaseList& getConstrainedByList(); @@ -105,6 +107,13 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma namespace wolf{ +inline void LandmarkBase::setProblem(ProblemPtr _problem) +{ + NodeBase::setProblem(_problem); + for (auto sb : state_block_vec_) + if (sb) sb->setProblem(_problem); +} + inline MapBasePtr LandmarkBase::getMapPtr() { return map_ptr_.lock(); diff --git a/src/node_base.h b/src/node_base.h index 6fc308280b803ffab21949f587ad50c8504dc78b..7ccb3bff2f53e13700b51fe2cfdbc9377e601527 100644 --- a/src/node_base.h +++ b/src/node_base.h @@ -81,7 +81,7 @@ class NodeBase void setName(const std::string& _name); ProblemPtr getProblem() const; - void setProblem(ProblemPtr _prob_ptr); + virtual void setProblem(ProblemPtr _prob_ptr); }; } // namespace wolf @@ -132,12 +132,12 @@ inline void NodeBase::setName(const std::string& _name) inline ProblemPtr NodeBase::getProblem() const { - return problem_ptr_.lock(); + return problem_ptr_.lock(); } inline void NodeBase::setProblem(ProblemPtr _prob_ptr) { - problem_ptr_ = _prob_ptr; + problem_ptr_ = _prob_ptr; } } // namespace wolf diff --git a/src/problem.cpp b/src/problem.cpp index 5e646aef19ee5e8f2e91c9fd47343c142f81f043..2b752a3697103e30f371b531e283326062989685 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -369,27 +369,20 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr) { //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl; + // set problem + _state_ptr->setProblem(shared_from_this()); + // add the state unit to the list state_block_list_.push_back(_state_ptr); - // queue for solver manager + // Add add notification _state_ptr->addNotification(StateBlock::Notification::ADD); - notified_state_block_list_.push_back(_state_ptr); - notified_state_block_list_.sort(); - notified_state_block_list_.unique(); - return _state_ptr; -} + // Push all notifications from SB to problem + for (auto notif : _state_ptr->getNotifications()) + notifyStateBlock(_state_ptr, notif); -void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr) -{ - //std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl; - - // queue for solver manager - _state_ptr->addNotification(StateBlock::Notification::FIX_UPDATE); - notified_state_block_list_.push_back(_state_ptr); - notified_state_block_list_.sort(); - notified_state_block_list_.unique(); + return _state_ptr; } void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr) @@ -401,6 +394,11 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr) // Add remove notification _state_ptr->addNotification(StateBlock::Notification::REMOVE); + notifyStateBlock(_state_ptr, StateBlock::Notification::REMOVE); +} + +void Problem::notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type) +{ notified_state_block_list_.push_back(_state_ptr); notified_state_block_list_.sort(); notified_state_block_list_.unique(); @@ -1367,4 +1365,6 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool print(0, constr_by, metric, state_blocks); } + + } // namespace wolf diff --git a/src/problem.h b/src/problem.h index 87ae8f4bdad357543d057f17d76a7829e74e9c10..d0e71b864b8c06262e43623517721637c418dc56 100644 --- a/src/problem.h +++ b/src/problem.h @@ -16,6 +16,7 @@ struct ProcessorParamsBase; //wolf includes #include "wolf.h" #include "frame_base.h" +#include "state_block.h" // std includes @@ -266,14 +267,14 @@ class Problem : public std::enable_shared_from_this<Problem> */ StateBlockPtr addStateBlock(StateBlockPtr _state_ptr); - /** \brief Adds a new state block to be updated to solver manager - */ - void updateStateBlockPtr(StateBlockPtr _state_ptr); - /** \brief Adds a state block to be removed to solver manager */ void removeStateBlockPtr(StateBlockPtr _state_ptr); + /** \brief Notify State Block change + */ + void notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type); + /** \brief Gets a list of state blocks which state has been changed to be handled by the solver */ StateBlockList& getNotifiedStateBlockList(); diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp index 00a4699ecc7c391a9b1c4b5aad2ad32f630df69e..e95769247f10e5e52219afcbb8692b7aa9c9de51 100644 --- a/src/processor_tracker_landmark_polyline.cpp +++ b/src/processor_tracker_landmark_polyline.cpp @@ -956,8 +956,8 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_ // Unfix origin polyline_ptr->getPPtr()->unfix(); polyline_ptr->getOPtr()->unfix(); - getProblem()->updateStateBlockPtr(polyline_ptr->getPPtr()); - getProblem()->updateStateBlockPtr(polyline_ptr->getOPtr()); + getProblem()->updateFixStateBlockPtr(polyline_ptr->getPPtr()); + getProblem()->updateFixStateBlockPtr(polyline_ptr->getOPtr()); // Move origin to B polyline_ptr->getPPtr()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id))); @@ -989,7 +989,7 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_ for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++) { polyline_ptr->getPointStateBlockPtr(id)->fix(); - getProblem()->updateStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id)); + getProblem()->updateFixStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id)); } } } diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 801922ff6acc3d43d44617a99563907a8dd6aabe..fbd05692eb2b7664f505f7bcccd8ef041952fe4e 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -111,11 +111,7 @@ void SensorBase::fix() { for( auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } updateCalibSize(); } @@ -123,11 +119,7 @@ void SensorBase::unfix() { for( auto sbp : state_block_vec_) if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } updateCalibSize(); } @@ -137,11 +129,7 @@ void SensorBase::fixExtrinsics() { auto sbp = state_block_vec_[i]; if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -152,11 +140,7 @@ void SensorBase::unfixExtrinsics() { auto sbp = state_block_vec_[i]; if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -167,11 +151,7 @@ void SensorBase::fixIntrinsics() { auto sbp = state_block_vec_[i]; if (sbp != nullptr) - { sbp->fix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -182,11 +162,7 @@ void SensorBase::unfixIntrinsics() { auto sbp = state_block_vec_[i]; if (sbp != nullptr) - { sbp->unfix(); - if (getProblem() != nullptr) - getProblem()->updateStateBlockPtr(sbp); - } } updateCalibSize(); } @@ -374,4 +350,13 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeSta return getStateBlockPtrStatic(_i); } +void SensorBase::setProblem(ProblemPtr _problem) +{ + NodeBase::setProblem(_problem); + for (auto prc : processor_list_) + prc->setProblem(_problem); + for (auto sb : state_block_vec_) + if (sb) sb->setProblem(_problem); +} + } // namespace wolf diff --git a/src/sensor_base.h b/src/sensor_base.h index f10b547f321e62d123f12ab6520d379e3df69dc8..3c740bc88233645d9d3d6767e65c048267761acc 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -95,6 +95,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa unsigned int id(); + virtual void setProblem(ProblemPtr _problem) final; + HardwareBasePtr getHardwarePtr(); void setHardwarePtr(const HardwareBasePtr _hw_ptr); @@ -163,8 +165,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa #include "problem.h" #include "hardware_base.h" -#include "processor_base.h" #include "capture_base.h" +#include "processor_base.h" namespace wolf{ diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 2914a4fc7216974f2701736d2ecb223dcfde3b1a..aec47485db1277e9ec150ddeeed5b33f2773419c 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -40,8 +40,6 @@ void SolverManager::update() { const bool registered = state_blocks_.find(state)!=state_blocks_.end(); - // const auto p = state_blocks_.emplace(state, state->getState()); - // call addStateBlock only if first time added. if (!registered) { @@ -53,20 +51,21 @@ void SolverManager::update() break; } -// case StateBlock::Notification::STATE_UPDATE: -// { -// WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(), -// "Updating the state of an unregistered StateBlock !"); -// -// assert(state_blocks_.find(state)!=state_blocks_.end() && -// "Updating the state of an unregistered StateBlock !"); -// -// Eigen::VectorXs new_state = state->getState(); -// std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state)); -// -// break; -// } - case StateBlock::Notification::FIX_UPDATE: + case StateBlock::Notification::UPDATE_STATE: + { + WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(), + "Updating the state of an unregistered StateBlock !"); + + assert(state_blocks_.find(state)!=state_blocks_.end() && + "Updating the state of an unregistered StateBlock !"); + + Eigen::VectorXs new_state = state->getState(); + // We assume the same size for the states in both WOLF and the solver. + std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state)); + + break; + } + case StateBlock::Notification::UPDATE_FIX: { WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(), "Updating the fix state of an unregistered StateBlock !"); @@ -156,7 +155,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level) { // Avoid usuless copies if (!it->first->isFixed()) - it->first->setState(it->second); + it->first->setState(it->second, false); } return report; diff --git a/src/state_block.cpp b/src/state_block.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2fee2da0e53adf92810f1ef421428410419a70f6 --- /dev/null +++ b/src/state_block.cpp @@ -0,0 +1,97 @@ +#include "state_block.h" +namespace wolf +{ + +void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify) +{ + assert(_state.size() == state_.size()); + { + std::lock_guard<std::mutex> lock(mut_state_); + state_ = _state; + state_size_ = state_.size(); + } + + // Notify + if (_notify) + { + addNotification(StateBlock::Notification::UPDATE_STATE); + if (getProblem() != nullptr) + getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_STATE); + } +} + +void StateBlock::setFixed(bool _fixed) +{ + fixed_.store(_fixed); + // Notify + addNotification(StateBlock::Notification::UPDATE_FIX); + if (getProblem() != nullptr) + getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_FIX); +} + +void StateBlock::addNotification(const StateBlock::Notification _new_notification) +{ + std::lock_guard<std::mutex> lock(notifictions_mut_); + if (_new_notification == Notification::ADD) + { + // When an ADD arrives, the state is already the newest, + // thus old instructions can be cleared + if (shared_from_this()->notifications_.size() > 0) + notifications_.clear(); + + // Push ADD notification to the front + notifications_.emplace_front(Notification::ADD); + } + else if (_new_notification == Notification::REMOVE) + { + // If we want to remove a block that still has an ADD instruction + // we can just clear all notifications and just keep the remove + if (!notifications_.empty() && notifications_.front() == Notification::ADD) + notifications_.clear(); + else + { + notifications_.clear(); + notifications_.emplace_back(Notification::REMOVE); + } + } + else + // UPDATE_FIX; UPDATE_STATE + notifications_.emplace_back(_new_notification); +} + +StateBlock::Notifications StateBlock::consumeNotifications() const +{ + std::lock_guard<std::mutex> lock(notifictions_mut_); + return std::move(notifications_); +} + +StateBlock::Notifications StateBlock::getNotifications() const +{ + return notifications_; +} + +void StateBlock::printNotifications() const +{ + WOLF_TRACE("SB Notifications for: ", shared_from_this()) + for (auto notif : notifications_) + { + switch (notif) + { + case Notification::ADD: + WOLF_TRACE(" ADD") + break; + case Notification::REMOVE: + WOLF_TRACE(" REMOVE") + break; + case Notification::UPDATE_FIX: + WOLF_TRACE(" UPDATE_FIX") + break; + case Notification::UPDATE_STATE: + WOLF_TRACE(" UPDATE_STATE") + break; + } + } + +} + +} diff --git a/src/state_block.h b/src/state_block.h index 6f99144e66cb784d9bb587163c779487ba4173e3..08d5af6591ab88de62ae4b0a7e2616a65f0b5351 100644 --- a/src/state_block.h +++ b/src/state_block.h @@ -28,16 +28,16 @@ namespace wolf { * - Non-fixed state blocks are estimated. * - A local parametrization useful for optimizing in the tangent space to the manifold. */ -class StateBlock +class StateBlock : public std::enable_shared_from_this<StateBlock> { public: enum class Notification : std::size_t { - ADD = 0, + ADD = 1, REMOVE, - STATE_UPDATE, - FIX_UPDATE + UPDATE_STATE, + UPDATE_FIX }; using Notifications = std::list<Notification>; @@ -47,7 +47,7 @@ public: mutable Notifications notifications_; mutable std::mutex notifictions_mut_; - NodeBaseWPtr node_ptr_; //< pointer to the wolf Node owning this StateBlock + ProblemWPtr problem_ptr_; ///< pointer to the wolf problem std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not @@ -89,7 +89,15 @@ public: /** \brief Sets the state vector **/ - void setState(const Eigen::VectorXs& _state); + void setState(const Eigen::VectorXs& _state, const bool _notify = true); + + /** \brief Set the pointer to Problem + */ + void setProblem(const ProblemPtr _problem); + + /** \brief Return the problem pointer + */ + ProblemPtr getProblem(); /** \brief Returns the state size **/ @@ -127,19 +135,31 @@ public: StateBlock::Notifications consumeNotifications() const; + /** \brief Check if exist any notification + **/ bool hasNotifications() const; + + /** \brief Return list of notifications + **/ + StateBlock::Notifications getNotifications() const; + + /** \brief Print list of notifications + **/ + void printNotifications() const; + }; } // namespace wolf // IMPLEMENTATION +#include "problem.h" #include "local_parametrization_base.h" +#include "node_base.h" namespace wolf { inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) : // notifications_{Notification::ADD}, - node_ptr_(), // nullptr fixed_(_fixed), state_size_(_state.size()), state_(_state), @@ -150,7 +170,6 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) : // notifications_{Notification::ADD}, - node_ptr_(), // nullptr fixed_(_fixed), state_size_(_size), state_(Eigen::VectorXs::Zero(_size)), @@ -171,19 +190,6 @@ inline Eigen::VectorXs StateBlock::getState() const return state_; } -inline void StateBlock::setState(const Eigen::VectorXs& _state) -{ - assert(_state.size() == state_.size()); - - { - std::lock_guard<std::mutex> lock(mut_state_); - state_ = _state; - state_size_ = state_.size(); - } - - //addNotification(Notification::STATE_UPDATE); -} - inline SizeEigen StateBlock::getSize() const { return state_size_.load(); @@ -211,12 +217,6 @@ inline void StateBlock::unfix() setFixed(false); } -inline void StateBlock::setFixed(bool _fixed) -{ - fixed_.store(_fixed); - addNotification(Notification::FIX_UPDATE); -} - inline bool StateBlock::hasLocalParametrization() const { return (local_param_ptr_ != nullptr); @@ -239,16 +239,14 @@ inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _ local_param_ptr_ = _local_param; } -inline void StateBlock::addNotification(const StateBlock::Notification _new_notification) +inline void StateBlock::setProblem(const ProblemPtr _problem) { - std::lock_guard<std::mutex> lock(notifictions_mut_); - notifications_.emplace_back(_new_notification); + problem_ptr_ = _problem; } -inline StateBlock::Notifications StateBlock::consumeNotifications() const +inline ProblemPtr StateBlock::getProblem() { - std::lock_guard<std::mutex> lock(notifictions_mut_); - return std::move(notifications_); + return problem_ptr_.lock(); } inline bool StateBlock::hasNotifications() const diff --git a/src/test/gtest_ceres_manager.cpp b/src/test/gtest_ceres_manager.cpp index 8f1466cb45d4d6844b91dd8d974c91846d563390..07ef3ab49101491c3396921f1907cb7dbe17dfb0 100644 --- a/src/test/gtest_ceres_manager.cpp +++ b/src/test/gtest_ceres_manager.cpp @@ -135,9 +135,6 @@ TEST(CeresManager, UpdateStateBlock) // Fix frame sb_ptr->fix(); - // update stateblock - P->updateStateBlockPtr(sb_ptr); - // update solver manager ceres_manager_ptr->update(); @@ -160,9 +157,6 @@ TEST(CeresManager, AddUpdateStateBlock) // Fix state block sb_ptr->fix(); - // update stateblock - P->updateStateBlockPtr(sb_ptr); - // update solver manager ceres_manager_ptr->update(); @@ -244,17 +238,6 @@ TEST(CeresManager, RemoveUpdateStateBlock) // update solver ceres_manager_ptr->update(); - - // Fix state block - sb_ptr->fix(); - - ASSERT_DEATH({ - // update stateblock - P->updateStateBlockPtr(sb_ptr); - - // update solver manager - ceres_manager_ptr->update(); - },""); } TEST(CeresManager, DoubleRemoveStateBlock) diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp index 7543ea18155e7fc80a79e079d3ac35d343335634..f2aee334e543ab02507c302d44a010555a70e4f1 100644 --- a/src/test/gtest_solver_manager.cpp +++ b/src/test/gtest_solver_manager.cpp @@ -80,7 +80,7 @@ TEST(SolverManager, Create) ProblemPtr P = Problem::create("PO 2D"); SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - // check double ointers to branches + // check double pointers to branches ASSERT_EQ(P, solver_manager_ptr->getProblemPtr()); } @@ -149,9 +149,6 @@ TEST(SolverManager, UpdateStateBlock) // Fix frame sb_ptr->fix(); - // update stateblock - P->updateStateBlockPtr(sb_ptr); - // update solver manager solver_manager_ptr->update(); @@ -174,9 +171,6 @@ TEST(SolverManager, AddUpdateStateBlock) // Fix state block sb_ptr->fix(); - // update stateblock - P->updateStateBlockPtr(sb_ptr); - // update solver manager solver_manager_ptr->update(); @@ -228,7 +222,7 @@ TEST(SolverManager, AddRemoveStateBlock) // update solver solver_manager_ptr->update(); - // check stateblock + // check state block ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); } @@ -241,7 +235,7 @@ TEST(SolverManager, RemoveUpdateStateBlock) Vector2s state; state << 1, 2; StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - // add stateblock + // add state_block P->addStateBlock(sb_ptr); // remove state_block @@ -249,17 +243,6 @@ TEST(SolverManager, RemoveUpdateStateBlock) // update solver solver_manager_ptr->update(); - - // Fix state block - sb_ptr->fix(); - - ASSERT_DEATH({ - // update stateblock - P->updateStateBlockPtr(sb_ptr); - - // update solver manager - solver_manager_ptr->update(); - },""); } TEST(SolverManager, DoubleRemoveStateBlock) @@ -287,6 +270,96 @@ TEST(SolverManager, DoubleRemoveStateBlock) solver_manager_ptr->update(); } +TEST(SolverManager, AddUpdatedStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Fix + sb_ptr->fix(); + + // Set State + Vector2s state_2 = 2*state; + sb_ptr->setState(state_2); + + // == First add 2 UPDATES (FIX and STATE) == + + // The expected order is: + // ADD in front of the list + // Others in historical order + ASSERT_EQ(sb_ptr->getNotifications().size(),2); // UPDATE_FIX, UPDATE_STATE + StateBlock::Notifications sb_notif = sb_ptr->getNotifications(); + StateBlock::Notifications::iterator iter = sb_notif.begin(); + ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_FIX); + iter++; + ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_STATE); + + // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) == + + // add state_block + P->addStateBlock(sb_ptr); + + // Get list of SB notifications + StateBlockList P_notif_sb = P->getNotifiedStateBlockList(); + + ASSERT_EQ(P_notif_sb.size(),1); + + sb_notif = P_notif_sb.front()->getNotifications(); + + ASSERT_EQ(sb_notif.size(),1); + ASSERT_EQ(sb_notif.front(),StateBlock::Notification::ADD); + + // == Insert OTHER notifications == + + // Set State + state_2 = 2*state; + sb_ptr->setState(state_2); + + // Fix + sb_ptr->unfix(); + + sb_notif = P_notif_sb.front()->getNotifications(); + ASSERT_EQ(sb_notif.size(),3); // ADD, UPDATE_STATE, UPDATE_FIX + + iter = sb_notif.begin(); + ASSERT_EQ(*iter,StateBlock::Notification::ADD); + iter++; + ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_STATE); + iter++; + ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_FIX); + + // == REMOVE should clear all previous notifications in the stack because there's an ADD == + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + P_notif_sb = P->getNotifiedStateBlockList(); + ASSERT_EQ(P_notif_sb.size(),1); + + sb_notif = P_notif_sb.front()->getNotifications(); + ASSERT_EQ(sb_notif.size(),0); // ADD + REMOVE = EMPTY + + // == UPDATES + REMOVE should clear the list of notifications == + + // Fix + sb_ptr->fix(); + + // Set State + state_2 = 2*state; + sb_ptr->setState(state_2); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + sb_notif = P_notif_sb.front()->getNotifications(); + ASSERT_EQ(sb_notif.size(),1); + ASSERT_EQ(sb_notif.front(),StateBlock::Notification::REMOVE); +} + TEST(SolverManager, AddConstraint) { ProblemPtr P = Problem::create("PO 2D"); diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp index 69a23a5bd184bc8b5f69e7885c2a69bb3b28a318..17e0667475716d16b694ee8c2b3d8139e91035af 100644 --- a/src/test/gtest_trajectory.cpp +++ b/src/test/gtest_trajectory.cpp @@ -46,11 +46,11 @@ struct DummyNotificationProcessor { break; } - case StateBlock::Notification::STATE_UPDATE: + case StateBlock::Notification::UPDATE_STATE: { break; } - case StateBlock::Notification::FIX_UPDATE: + case StateBlock::Notification::UPDATE_FIX: { break; }