diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index f307f2bd561e3b30db1a232c45f4792b4a9e44e7..d5d76ba8e37c92d2d17a00cf40b59fea18157b33 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -61,7 +61,6 @@ CaptureBase::CaptureBase(const std::string& _type,
}
-
CaptureBase::~CaptureBase()
{
removeStateBlocks();
@@ -169,7 +168,7 @@ void CaptureBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlockPtr(sbp);
+ getProblem()->removeStateBlock(sbp);
}
setStateBlockPtr(i, nullptr);
}
diff --git a/src/capture_base.h b/src/capture_base.h
index 3c14dc5e1cd96908f91e45d81763e2191eae5bf1..06e8541bafb3eff92903e1b982c86dabdcc087f2 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -125,8 +125,6 @@ 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()
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 59252fbbdc6a0ec80f1bb3ed91c02c5f8b51a44d..bbd383c66f3ea4edbe0a3caadeebbd5af47d2a02 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -303,6 +303,7 @@ void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
assert(state_ptr);
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+ state_blocks_local_param_.erase(state_ptr);
}
void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
@@ -314,6 +315,42 @@ void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
}
+void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+{
+ assert(state_ptr != nullptr);
+
+ /* in ceres the easiest way to update (add or remove) a local parameterization
+ * of a state block (parameter block in ceres) is remove & add:
+ * - the state block: The associated memory block (that identified the parameter_block) is and MUST be the same
+ * - all involved constraints (residual_blocks in ceres)
+ */
+
+ // get all involved constraints
+ ConstraintBaseList involved_constraints;
+ for (auto pair : ctr_2_costfunction_)
+ for (const StateBlockPtr& st : pair.first->getStateBlockPtrVector())
+ if (st == state_ptr)
+ {
+ // store
+ involved_constraints.push_back(pair.first);
+ break;
+ }
+
+ // Remove all involved constraints (it does not remove any parameter block)
+ for (auto ctr : involved_constraints)
+ removeConstraint(ctr);
+
+ // Remove state block (it removes all involved residual blocks but they just were removed)
+ removeStateBlock(state_ptr);
+
+ // Add state block
+ addStateBlock(state_ptr);
+
+ // Add all involved constraints
+ for (auto ctr : involved_constraints)
+ addConstraint(ctr);
+}
+
ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
{
assert(_ctr_ptr != nullptr);
@@ -349,7 +386,7 @@ void CeresManager::check()
assert(ctr_2_costfunction_[ctr_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second));
// constraint - residual
- assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->constraint_ptr_);
+ assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->getConstraintPtr());
// parameter blocks - state blocks
std::vector<Scalar*> param_blocks;
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 3311b1c0761146d75456be3909aa881c993fffa8..ebc8b04e945fb3007aa1e6c6926b450c84211746 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -55,6 +55,8 @@ class CeresManager : public SolverManager
ceres::Solver::Options& getSolverOptions();
+ void check();
+
private:
std::string solveImpl(const ReportVerbosity report_level) override;
@@ -69,9 +71,9 @@ class CeresManager : public SolverManager
void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
- ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+ void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override;
- void check();
+ ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
};
inline ceres::Solver::Summary CeresManager::getSummary()
diff --git a/src/ceres_wrapper/cost_function_wrapper.h b/src/ceres_wrapper/cost_function_wrapper.h
index 65566f2fa314c6a37024e8335990210f347cd437..ffd88191475c23ae409651baedb0099f3aaa4064 100644
--- a/src/ceres_wrapper/cost_function_wrapper.h
+++ b/src/ceres_wrapper/cost_function_wrapper.h
@@ -17,14 +17,19 @@ WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
class CostFunctionWrapper : public ceres::CostFunction
{
- public:
+ private:
+
ConstraintBasePtr constraint_ptr_;
+ public:
+
CostFunctionWrapper(ConstraintBasePtr _constraint_ptr);
virtual ~CostFunctionWrapper();
virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
+
+ ConstraintBasePtr getConstraintPtr() const;
};
inline CostFunctionWrapper::CostFunctionWrapper(ConstraintBasePtr _constraint_ptr) :
@@ -44,6 +49,11 @@ inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, doub
return constraint_ptr_->evaluate(parameters, residuals, jacobians);
}
+inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const
+{
+ return constraint_ptr_;
+}
+
} // namespace wolf
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.h b/src/ceres_wrapper/local_parametrization_wrapper.h
index 1d81970a963c374cf91a5b283cf443774cb05729..d9e0d9e82671ceb232e47760e2f74a119304ff3e 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.h
+++ b/src/ceres_wrapper/local_parametrization_wrapper.h
@@ -30,6 +30,8 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
virtual int GlobalSize() const;
virtual int LocalSize() const;
+
+ LocalParametrizationBasePtr getLocalParametrizationPtr() const;
};
using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
@@ -55,6 +57,11 @@ inline int LocalParametrizationWrapper::LocalSize() const
return local_parametrization_ptr_->getLocalSize();
}
+inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrizationPtr() const
+{
+ return local_parametrization_ptr_;
+}
+
} // namespace wolf
#endif
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index ca54791bb95766f72ab7d9e67eb7d717cb8dd081..d88dedbf05f89fcf7386b447520c31a4752a71a0 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -60,7 +60,7 @@ void ConstraintBase::remove()
}
// add constraint to be removed from solver
if (getProblem() != nullptr)
- getProblem()->removeConstraintPtr(shared_from_this());
+ getProblem()->removeConstraint(shared_from_this());
// remove other: {Frame, Capture, Feature, Landmark}
FrameBasePtr frm_o = frame_other_ptr_.lock();
@@ -126,9 +126,9 @@ void ConstraintBase::setStatus(ConstraintStatus _status)
else if (_status != status_)
{
if (_status == CTR_ACTIVE)
- getProblem()->addConstraintPtr(shared_from_this());
+ getProblem()->addConstraint(shared_from_this());
else if (_status == CTR_INACTIVE)
- getProblem()->removeConstraintPtr(shared_from_this());
+ getProblem()->removeConstraint(shared_from_this());
}
status_ = _status;
}
@@ -142,8 +142,8 @@ void ConstraintBase::setApplyLossFunction(const bool _apply)
else
{
ConstraintBasePtr this_c = shared_from_this();
- getProblem()->removeConstraintPtr(this_c);
- getProblem()->addConstraintPtr(this_c);
+ getProblem()->removeConstraint(this_c);
+ getProblem()->addConstraint(this_c);
}
}
}
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index eb3351129c6b42a51aeef0e2fa98c2fff18df6b1..3cc0b6ca3d64b7299641de401fb67068c0af1b08 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -61,7 +61,7 @@ ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
if (getProblem() != nullptr)
{
if (_co_ptr->getStatus() == CTR_ACTIVE)
- getProblem()->addConstraintPtr(_co_ptr);
+ getProblem()->addConstraint(_co_ptr);
}
else
WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 453e54cb58a5c11297866da827539b640c0e2c59..c605ae015c384e235a5409d215a3b2903c1bb439 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -106,7 +106,7 @@ void FrameBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlockPtr(sbp);
+ getProblem()->removeStateBlock(sbp);
}
setStateBlockPtr(i, nullptr);
}
diff --git a/src/frame_base.h b/src/frame_base.h
index 8f49e9b70a9275b590466fe5edcc728cb731605b..d5202bb69e636d7b3f9a8509a0af2ebcb57b6f28 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -265,8 +265,6 @@ 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()
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index b84c078162635f2320abe46abd6e4c8a3c93cc61..750911457c058acf179d635d24bc289df8237bad 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -114,7 +114,7 @@ void LandmarkBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlockPtr(sbp);
+ getProblem()->removeStateBlock(sbp);
}
setStateBlockPtr(i, nullptr);
}
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 5bc32c90083aacb06edf3ce160c54bdc90334c76..e4b721db95a7fe30db760f3a517a10e11f4f6872 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -110,8 +110,6 @@ 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()
diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index d117754206d53c9a409e5e04e6e92548a0249d12..f386803187e25ac323fe85ef6e352b0f0e7de956 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -170,7 +170,7 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
// remove and add state block without local parameterization
if (getProblem() != nullptr)
- getProblem()->removeStateBlockPtr(state);
+ getProblem()->removeStateBlock(state);
state->removeLocalParametrization();
@@ -182,8 +182,8 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
for (auto st_ptr : ctr_ptr->getStateBlockPtrVector())
if (st_ptr == state && getProblem() != nullptr)
{
- getProblem()->removeConstraintPtr(ctr_ptr);
- getProblem()->addConstraintPtr(ctr_ptr);
+ getProblem()->removeConstraint(ctr_ptr);
+ getProblem()->addConstraint(ctr_ptr);
}
// update boolean
@@ -304,7 +304,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// Remove remove_state
if (getProblem() != nullptr)
- getProblem()->removeStateBlockPtr(remove_state);
+ getProblem()->removeStateBlock(remove_state);
std::cout << "state removed " << std::endl;
// remove element from deque
@@ -329,7 +329,7 @@ void LandmarkPolyline2D::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlockPtr(sbp);
+ getProblem()->removeStateBlock(sbp);
}
point_state_ptr_vector_[i] = nullptr;
}
diff --git a/src/problem.cpp b/src/problem.cpp
index 8eba70e3f585adb283d479742eb985c61bbcdde9..0b68ffd852a61997178de317f5d12004bd779d90 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -368,66 +368,97 @@ void Problem::addLandmarkList(LandmarkBaseList& _lmk_list)
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
-
- // set problem
- _state_ptr->setProblem(shared_from_this());
+ if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
+ {
+ WOLF_WARN("Adding a state block that has already been added");
+ return _state_ptr;
+ }
// add the state unit to the list
state_block_list_.push_back(_state_ptr);
// Add add notification
- _state_ptr->addNotification(StateBlock::Notification::ADD);
-
- // Push all notifications from SB to problem
- notifyStateBlock(_state_ptr);
+ // Check if there is already a notification for this state block
+ auto notification_it = state_block_notification_map_.find(_state_ptr);
+ if (notification_it != state_block_notification_map_.end() && notification_it->second == ADD)
+ {
+ WOLF_WARN("There is already an ADD notification of this state block");
+ }
+ else
+ state_block_notification_map_[_state_ptr] = ADD;
return _state_ptr;
}
-void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
+void Problem::removeStateBlock(StateBlockPtr _state_ptr)
{
//std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
+ //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
+ if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
+ {
+ WOLF_WARN("Removing a state_block that hasn't been added or already removed");
+ return;
+ }
// add the state unit to the list
state_block_list_.remove(_state_ptr);
- // Add remove notification
- _state_ptr->addNotification(StateBlock::Notification::REMOVE);
- notifyStateBlock(_state_ptr);
-}
-
-void Problem::notifyStateBlock(StateBlockPtr _state_ptr)
-{
- notified_state_block_list_.push_back(_state_ptr);
- notified_state_block_list_.sort();
- notified_state_block_list_.unique();
+ // Check if there is already a notification for this state block
+ auto notification_it = state_block_notification_map_.find(_state_ptr);
+ if (notification_it != state_block_notification_map_.end())
+ {
+ if (notification_it->second == REMOVE)
+ {
+ WOLF_WARN("There is already an REMOVE notification of this state block");
+ }
+ // Remove ADD notification
+ else
+ {
+ state_block_notification_map_.erase(notification_it);
+ }
+ }
+ // Add REMOVE notification
+ else
+ state_block_notification_map_[_state_ptr] = REMOVE;
}
-ConstraintBasePtr Problem::addConstraintPtr(ConstraintBasePtr _constraint_ptr)
+ConstraintBasePtr Problem::addConstraint(ConstraintBasePtr _constraint_ptr)
{
//std::cout << "Problem::addConstraintPtr " << _constraint_ptr->id() << std::endl;
- // queue for solver manager
- constraint_notification_list_.push_back(ConstraintNotification({ADD, _constraint_ptr}));
+
+ // Add ADD notification
+ // Check if there is already a notification for this state block
+ auto notification_it = constraint_notification_map_.find(_constraint_ptr);
+ if (notification_it != constraint_notification_map_.end() && notification_it->second == ADD)
+ {
+ WOLF_WARN("There is already an ADD notification of this constraint");
+ }
+ // Add ADD notification (override in case of REMOVE)
+ else
+ constraint_notification_map_[_constraint_ptr] = ADD;
return _constraint_ptr;
}
-void Problem::removeConstraintPtr(ConstraintBasePtr _constraint_ptr)
+void Problem::removeConstraint(ConstraintBasePtr _constraint_ptr)
{
//std::cout << "Problem::removeConstraintPtr " << _constraint_ptr->id() << std::endl;
- // Check if the constraint addition is still as a notification
- auto ctr_notif_it = constraint_notification_list_.begin();
- for (; ctr_notif_it != constraint_notification_list_.end(); ctr_notif_it++)
- if (ctr_notif_it->notification_ == ADD && ctr_notif_it->constraint_ptr_ == _constraint_ptr)
- break;
-
- // Remove addition notification
- if (ctr_notif_it != constraint_notification_list_.end())
- constraint_notification_list_.erase(ctr_notif_it); // CHECKED shared_ptr is not active after erase
- // Add remove notification
+ // Check if there is already a notification for this state block
+ auto notification_it = constraint_notification_map_.find(_constraint_ptr);
+ if (notification_it != constraint_notification_map_.end())
+ {
+ if (notification_it->second == REMOVE)
+ {
+ WOLF_WARN("There is already an REMOVE notification of this state block");
+ }
+ // Remove ADD notification
+ else
+ constraint_notification_map_.erase(notification_it);
+ }
+ // Add REMOVE notification
else
- constraint_notification_list_.push_back(ConstraintNotification({REMOVE, _constraint_ptr}));
+ constraint_notification_map_[_constraint_ptr] = REMOVE;
}
void Problem::clearCovariance()
diff --git a/src/problem.h b/src/problem.h
index a8a182a04ba2b7d824663cc8d8320c66db4bddaa..3b5988333bcd7474a90b67bd5013c1863ae4b0f9 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -27,17 +27,9 @@ namespace wolf {
enum Notification
{
ADD,
- REMOVE,
- UPDATE
+ REMOVE
};
-struct ConstraintNotification
-{
- Notification notification_;
- ConstraintBasePtr constraint_ptr_;
-};
-
-
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
@@ -50,9 +42,9 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorMotionPtr processor_motion_ptr_;
StateBlockList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
- std::list<ConstraintNotification> constraint_notification_list_;
SizeEigen state_size_, state_cov_size_;
- StateBlockList notified_state_block_list_;
+ std::map<ConstraintBasePtr, Notification> constraint_notification_map_;
+ std::map<StateBlockPtr, Notification> state_block_notification_map_;
bool prior_is_set_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -259,37 +251,33 @@ class Problem : public std::enable_shared_from_this<Problem>
// Solver management ----------------------------------------
- /** \brief Gets a reference to the state units list
+ /** \brief Gets a reference to the state blocks list
*/
StateBlockList& getStateBlockList();
- /** \brief Adds a new state block to be added to solver manager
+ /** \brief Notifies a new state block to be added to the solver manager
*/
StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
- /** \brief Adds a state block to be removed to solver manager
- */
- void removeStateBlockPtr(StateBlockPtr _state_ptr);
-
- /** \brief Notify State Block change
+ /** \brief Notifies a state block to be removed from the solver manager
*/
- void notifyStateBlock(StateBlockPtr _state_ptr);
+ void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
+ /** \brief Gets a map of constraint notification to be handled by the solver
*/
- StateBlockList& getNotifiedStateBlockList();
+ std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
- /** \brief Gets a queue of constraint notification to be handled by the solver
+ /** \brief Notifies a new constraint to be added to the solver manager
*/
- std::list<ConstraintNotification>& getConstraintNotificationList();
+ ConstraintBasePtr addConstraint(ConstraintBasePtr _constraint_ptr);
- /** \brief Adds a new constraint to be added to solver manager
+ /** \brief Notifies a constraint to be removed from the solver manager
*/
- ConstraintBasePtr addConstraintPtr(ConstraintBasePtr _constraint_ptr);
+ void removeConstraint(ConstraintBasePtr _constraint_ptr);
- /** \brief Adds a constraint to be removed to solver manager
+ /** \brief Gets a map of constraint notification to be handled by the solver
*/
- void removeConstraintPtr(ConstraintBasePtr _constraint_ptr);
+ std::map<ConstraintBasePtr, Notification>& getConstraintNotificationMap();
// Print and check ---------------------------------------
/**
@@ -328,14 +316,14 @@ inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
return processor_motion_ptr_;
}
-inline std::list<ConstraintNotification>& Problem::getConstraintNotificationList()
+inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationMap()
{
- return constraint_notification_list_;
+ return state_block_notification_map_;
}
-inline StateBlockList& Problem::getNotifiedStateBlockList()
+inline std::map<ConstraintBasePtr,Notification>& Problem::getConstraintNotificationMap()
{
- return notified_state_block_list_;
+ return constraint_notification_map_;
}
} // namespace wolf
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index fbd05692eb2b7664f505f7bcccd8ef041952fe4e..7d36d7bf8284f9836cd7d4ee97a00c82665e05cd 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -98,7 +98,7 @@ void SensorBase::removeStateBlocks()
{
if (getProblem() != nullptr && !extrinsic_dynamic_)
{
- getProblem()->removeStateBlockPtr(sbp);
+ getProblem()->removeStateBlock(sbp);
}
setStateBlockPtrStatic(i, nullptr);
}
@@ -355,8 +355,6 @@ 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/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 8f08f8ae81744c6491285848c42b5dfbf3874056..b6a2e6a40e160a7604063de97ed811933d39e2dd 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -14,124 +14,100 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
// REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
- while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
+ auto ctr_notification_it = wolf_problem_->getConstraintNotificationMap().begin();
+ while ( ctr_notification_it != wolf_problem_->getConstraintNotificationMap().end() )
{
- if (ctr_notification_it->notification_ == REMOVE)
+ if (ctr_notification_it->second == REMOVE)
{
- removeConstraint(ctr_notification_it->constraint_ptr_);
- ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
+ removeConstraint(ctr_notification_it->first);
+ ctr_notification_it = wolf_problem_->getConstraintNotificationMap().erase(ctr_notification_it);
}
else
ctr_notification_it++;
}
- StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
-
- for (StateBlockPtr& state : states)
+ // ADD/REMOVE STATE BLOCS
+ auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
+ while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
{
- const auto notifications = state->consumeNotifications();
+ StateBlockPtr state_ptr = sb_notification_it->first;
- for (const auto notif : notifications)
+ if (sb_notification_it->second == ADD)
{
- switch (notif)
+ // only add if not added
+ if (state_blocks_.find(state_ptr) == state_blocks_.end())
+ {
+ state_blocks_.emplace(state_ptr, state_ptr->getState());
+ addStateBlock(state_ptr);
+ // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+ }
+ else
{
- case StateBlock::Notification::ADD:
- {
- const bool registered = state_blocks_.find(state)!=state_blocks_.end();
-
- // call addStateBlock only if first time added.
- if (!registered)
- {
- state_blocks_.emplace(state, state->getState());
- addStateBlock(state);
- }
-
- WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
-
- break;
- }
- 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 !");
-
- assert(state_blocks_.find(state)!=state_blocks_.end() &&
- "Updating the fix state of an unregistered StateBlock !");
-
- if (state_blocks_.find(state)!=state_blocks_.end())
- {
- updateStateBlockStatus(state);
- }
-
- break;
- }
- case StateBlock::Notification::REMOVE:
- {
- WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
- "Tried to remove a StateBlock that was not added !");
-
- if (state_blocks_.find(state)!=state_blocks_.end())
- {
- removeStateBlock(state);
- state_blocks_.erase(state);
- }
-
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: State Block notification "
- "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
+ WOLF_DEBUG("Tried adding an already registered StateBlock.");
}
}
- }
-
- states.clear();
-
- // ADD CONSTRAINTS
- while (!wolf_problem_->getConstraintNotificationList().empty())
- {
- switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+ else
{
- case Notification::ADD:
+ // only remove if it exists
+ if (state_blocks_.find(state_ptr)!=state_blocks_.end())
{
- addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
-
- break;
+ removeStateBlock(state_ptr);
+ state_blocks_.erase(state_ptr);
+ }
+ else
+ {
+ WOLF_DEBUG("Tried to remove a StateBlock that was not added !");
}
- default:
- throw std::runtime_error("SolverManager::update:"
- " Constraint notification must be ADD or REMOVE.");
}
-
- wolf_problem_->getConstraintNotificationList().pop_front();
+ // next notification
+ sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
}
- assert(wolf_problem_->getConstraintNotificationList().empty() &&
- "wolf problem's constraints notification list not empty after update");
- assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
- "wolf problem's state_blocks notification list not empty after update");
+ // ADD CONSTRAINTS
+ ctr_notification_it = wolf_problem_->getConstraintNotificationMap().begin();
+ while (ctr_notification_it != wolf_problem_->getConstraintNotificationMap().end())
+ {
+ assert(wolf_problem_->getConstraintNotificationMap().begin()->second == ADD && "unexpected constraint notification value after all REMOVE have been processed, this should be ADD");
+
+ addConstraint(wolf_problem_->getConstraintNotificationMap().begin()->first);
+ ctr_notification_it = wolf_problem_->getConstraintNotificationMap().erase(ctr_notification_it);
+ }
- // UPDATE ALL STATES
- for (StateBlockPtr& state : states)
+ // UPDATE STATE BLOCKS (state, fix or local parameterization)
+ for (auto state_ptr : wolf_problem_->getStateBlockList())
{
- Eigen::VectorXs new_state = state->getState();
- std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
+ assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
+
+ // state update
+ if (state_ptr->stateUpdated())
+ {
+ Eigen::VectorXs new_state = state_ptr->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_ptr));
+ // reset flag
+ state_ptr->resetStateUpdated();
+ }
+ // fix update
+ if (state_ptr->fixUpdated())
+ {
+ updateStateBlockStatus(state_ptr);
+ // reset flag
+ state_ptr->resetFixUpdated();
+ }
+ // local parameterization update
+ if (state_ptr->localParamUpdated())
+ {
+ updateStateBlockLocalParametrization(state_ptr);
+ // reset flag
+ state_ptr->resetLocalParamUpdated();
+ }
}
+
+ assert(wolf_problem_->getConstraintNotificationMap().empty() && "wolf problem's constraints notification map not empty after update");
+ assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
wolf::ProblemPtr SolverManager::getProblemPtr()
@@ -148,6 +124,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
+ /// JV: I do not see a problem here, the solver provides the optimal state given the constraints, if someone changed the state during optimization, it will be overwritten by the optimal one.
std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
it_end = state_blocks_.end();
@@ -155,7 +132,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
{
// Avoid usuless copies
if (!it->first->isFixed())
- it->first->setState(it->second, false);
+ it->first->setState(it->second, false); // false = do not raise the flag state_updated_
}
return report;
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
index 51325380a31c9d10cba2a3039c62f2216a5b0706..5416067acf84572424f9017c606ab0aa910e92b3 100644
--- a/src/solver/solver_manager.h
+++ b/src/solver/solver_manager.h
@@ -77,6 +77,8 @@ protected:
virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
+
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) = 0;
};
} // namespace wolf
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 3e53245de9c78b5720d56822c22d4e45447832f0..2599ea21c66351c6a74d40cf4b05bd9f2161d3c4 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -11,65 +11,29 @@ void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
state_size_ = state_.size();
}
- // Notify
+ // Flag
if (_notify)
- {
- addNotification(StateBlock::Notification::UPDATE_STATE);
- if (getProblem() != nullptr)
- getProblem()->notifyStateBlock(shared_from_this());
- }
+ state_updated_.store(true);
}
void StateBlock::setFixed(bool _fixed)
{
- fixed_.store(_fixed);
- // Notify
- addNotification(StateBlock::Notification::UPDATE_FIX);
- if (getProblem() != nullptr)
- getProblem()->notifyStateBlock(shared_from_this());
-}
+ // Flag
+ if (fixed_.load() != _fixed)
+ fix_updated_.store(true);
-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
-{
- std::lock_guard<std::mutex> lock(notifictions_mut_);
- return notifications_;
+ // set
+ fixed_.store(_fixed);
}
+//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
+//{
+// _problem_ptr->addStateBlock(shared_from_this());
+//}
+//
+//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
+//{
+// _problem_ptr->removeStateBlockPtr(shared_from_this());
+//}
}
diff --git a/src/state_block.h b/src/state_block.h
index d2d7c3159d5c0e75d0b7778cbccff4a5601348ac..cdfc94c24dbc488aa8a299c5f058aa263f049924 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -10,13 +10,11 @@ class LocalParametrizationBase;
//Wolf includes
#include "wolf.h"
-#include "local_parametrization_base.h"
//std includes
#include <iostream>
#include <mutex>
-
namespace wolf {
/** \brief class StateBlock
@@ -32,31 +30,20 @@ class StateBlock : public std::enable_shared_from_this<StateBlock>
{
public:
- enum class Notification : std::size_t
- {
- ADD = 1,
- REMOVE,
- UPDATE_STATE,
- UPDATE_FIX
- };
-
- using Notifications = std::list<Notification>;
-
protected:
- mutable Notifications notifications_;
- mutable std::mutex notifictions_mut_;
-
- ProblemWPtr problem_ptr_; ///< pointer to the wolf problem
+ std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
- std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
-
- std::atomic<SizeEigen> state_size_; ///< State vector size
- Eigen::VectorXs state_; ///< State vector storing the state values
- mutable std::mutex mut_state_; ///< State vector mutex
+ std::atomic<SizeEigen> state_size_; ///< State vector size
+ Eigen::VectorXs state_; ///< State vector storing the state values
+ mutable std::mutex mut_state_; ///< State vector mutex
LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
+ std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
+ std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
+ std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
+
public:
/** \brief Constructor from size
*
@@ -91,21 +78,11 @@ public:
**/
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
**/
SizeEigen getSize() const;
/**\brief Returns the size of the local parametrization
- *
- * @return the size of the local parametrization
*/
SizeEigen getLocalSize() const;
@@ -121,36 +98,65 @@ public:
**/
void unfix();
+ /** \brief Sets the state status
+ **/
void setFixed(bool _fixed);
+ /** \brief Returns if the state has a local parametrization
+ **/
bool hasLocalParametrization() const;
+ /** \brief Returns the state local parametrization ptr
+ **/
LocalParametrizationBasePtr getLocalParametrizationPtr() const;
+ /** \brief Sets a local parametrization
+ **/
void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
+ /** \brief Removes the state_block local parametrization
+ **/
void removeLocalParametrization();
- void addNotification(const StateBlock::Notification _new_notification);
+ /** \brief Return if state has been updated
+ **/
+ bool stateUpdated() const;
+
+ /** \brief Return if fix/unfix has been updated
+ **/
+ bool fixUpdated() const;
+
+ /** \brief Return if local parameterization has been updated
+ **/
+ bool localParamUpdated() const;
+
+ /** \brief Set state_updated_ to false
+ **/
+ void resetStateUpdated();
- StateBlock::Notifications consumeNotifications() const;
+ /** \brief Set fix_updated_ to false
+ **/
+ void resetFixUpdated();
- /** \brief Check if exist any notification
+ /** \brief Set localk_param_updated_ to false
**/
- bool hasNotifications() const;
+ void resetLocalParamUpdated();
- /** \brief Return list of notifications
+ /** \brief Add this state_block to the problem
**/
- StateBlock::Notifications getNotifications() const;
+ //void addToProblem(ProblemPtr _problem_ptr);
+ /** \brief Remove this state_block from the problem
+ **/
+ //void removeFromProblem(ProblemPtr _problem_ptr);
};
} // namespace wolf
// IMPLEMENTATION
-#include "problem.h"
#include "local_parametrization_base.h"
#include "node_base.h"
+#include "problem.h"
namespace wolf {
@@ -159,7 +165,10 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP
fixed_(_fixed),
state_size_(_state.size()),
state_(_state),
- local_param_ptr_(_local_param_ptr)
+ local_param_ptr_(_local_param_ptr),
+ state_updated_(false),
+ fix_updated_(false),
+ local_param_updated_(false)
{
// std::cout << "constructed +sb" << std::endl;
}
@@ -169,7 +178,10 @@ inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametri
fixed_(_fixed),
state_size_(_size),
state_(Eigen::VectorXs::Zero(_size)),
- local_param_ptr_(_local_param_ptr)
+ local_param_ptr_(_local_param_ptr),
+ state_updated_(false),
+ fix_updated_(false),
+ local_param_updated_(false)
{
//
// std::cout << "constructed +sb" << std::endl;
@@ -227,30 +239,46 @@ inline void StateBlock::removeLocalParametrization()
{
assert(local_param_ptr_ != nullptr && "state block without local parametrization");
local_param_ptr_.reset();
+ local_param_updated_.store(true);
}
inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param)
{
assert(_local_param != nullptr && "setting a null local parametrization");
local_param_ptr_ = _local_param;
+ local_param_updated_.store(true);
}
-inline void StateBlock::setProblem(const ProblemPtr _problem)
+inline bool StateBlock::stateUpdated() const
{
- problem_ptr_ = _problem;
+ return state_updated_.load();
}
-inline ProblemPtr StateBlock::getProblem()
+inline bool StateBlock::fixUpdated() const
{
- return problem_ptr_.lock();
+ return fix_updated_.load();
}
-inline bool StateBlock::hasNotifications() const
+inline bool StateBlock::localParamUpdated() const
{
- std::lock_guard<std::mutex> lock(notifictions_mut_);
- return !notifications_.empty();
+ return local_param_updated_.load();
}
-} // namespace wolf
+inline void StateBlock::resetStateUpdated()
+{
+ state_updated_.store(false);
+}
+
+inline void StateBlock::resetFixUpdated()
+{
+ fix_updated_.store(false);
+}
+
+inline void StateBlock::resetLocalParamUpdated()
+{
+ local_param_updated_.store(false);
+}
+
+}// namespace wolf
#endif
diff --git a/src/test/gtest_ceres_manager.cpp b/src/test/gtest_ceres_manager.cpp
index 07ef3ab49101491c3396921f1907cb7dbe17dfb0..0df86c75e583f51f49a4dd918c79337be92a5e64 100644
--- a/src/test/gtest_ceres_manager.cpp
+++ b/src/test/gtest_ceres_manager.cpp
@@ -13,8 +13,11 @@
#include "../state_block.h"
#include "../capture_void.h"
#include "../constraint_pose_2D.h"
+#include "../constraint_quaternion_absolute.h"
#include "../solver/solver_manager.h"
#include "../ceres_wrapper/ceres_manager.h"
+#include "../local_parametrization_angle.h"
+#include "../local_parametrization_quaternion.h"
#include "ceres/ceres.h"
@@ -52,11 +55,28 @@ class CeresManagerWrapper : public CeresManager
return ceres_problem_->NumParameterBlocks();
};
+ int numConstraints()
+ {
+ return ceres_problem_->NumResidualBlocks();
+ };
+
bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const
{
return ctr_2_residual_idx_.find(ctr_ptr) != ctr_2_residual_idx_.end() && ctr_2_costfunction_.find(ctr_ptr) != ctr_2_costfunction_.end();
};
+ bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+ {
+ return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
+ state_blocks_local_param_.at(st)->getLocalParametrizationPtr() == local_param &&
+ ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
+ };
+
+ bool hasLocalParametrization(const StateBlockPtr& st) const
+ {
+ return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
+ };
+
};
TEST(CeresManager, Create)
@@ -66,6 +86,9 @@ TEST(CeresManager, Create)
// check double ointers to branches
ASSERT_EQ(P, ceres_manager_ptr->getProblemPtr());
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, AddStateBlock)
@@ -86,6 +109,9 @@ TEST(CeresManager, AddStateBlock)
// check stateblock
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, DoubleAddStateBlock)
@@ -112,6 +138,9 @@ TEST(CeresManager, DoubleAddStateBlock)
// check stateblock
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, UpdateStateBlock)
@@ -140,6 +169,9 @@ TEST(CeresManager, UpdateStateBlock)
// check stateblock fixed
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, AddUpdateStateBlock)
@@ -164,6 +196,9 @@ TEST(CeresManager, AddUpdateStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, RemoveStateBlock)
@@ -185,14 +220,17 @@ TEST(CeresManager, RemoveStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
ceres_manager_ptr->update();
// check stateblock
ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0);
+ ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, AddRemoveStateBlock)
@@ -208,14 +246,17 @@ TEST(CeresManager, AddRemoveStateBlock)
P->addStateBlock(sb_ptr);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
ceres_manager_ptr->update();
// check no stateblocks
ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0);
+ ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, RemoveUpdateStateBlock)
@@ -234,10 +275,13 @@ TEST(CeresManager, RemoveUpdateStateBlock)
ceres_manager_ptr->update();
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
ceres_manager_ptr->update();
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, DoubleRemoveStateBlock)
@@ -253,16 +297,19 @@ TEST(CeresManager, DoubleRemoveStateBlock)
P->addStateBlock(sb_ptr);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
ceres_manager_ptr->update();
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver manager
ceres_manager_ptr->update();
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, AddConstraint)
@@ -270,9 +317,27 @@ TEST(CeresManager, AddConstraint)
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ // Create (and add) constraint point 2d
+ FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+ FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+ ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check constraint
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 1);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+TEST(CeresManager, DoubleAddConstraint)
+{
+ ProblemPtr P = Problem::create("PO 2D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) constraint point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
@@ -280,11 +345,18 @@ TEST(CeresManager, AddConstraint)
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ // add constraint again
+ P->addConstraint(c);
+
// update solver
ceres_manager_ptr->update();
// check constraint
ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 1);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, RemoveConstraint)
@@ -292,10 +364,6 @@ TEST(CeresManager, RemoveConstraint)
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
// Create (and add) constraint point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
@@ -305,14 +373,18 @@ TEST(CeresManager, RemoveConstraint)
// update solver
ceres_manager_ptr->update();
- // add constraint
- P->removeConstraintPtr(c);
+ // remove constraint
+ P->removeConstraint(c);
// update solver
ceres_manager_ptr->update();
// check constraint
ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, AddRemoveConstraint)
@@ -320,28 +392,28 @@ TEST(CeresManager, AddRemoveConstraint)
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
// Create (and add) constraint point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
- ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c);
+ ASSERT_TRUE(P->getConstraintNotificationMap().begin()->first == c);
- // add constraint
- P->removeConstraintPtr(c);
+ // remove constraint
+ P->removeConstraint(c);
- ASSERT_TRUE(P->getConstraintNotificationList().empty());
+ ASSERT_TRUE(P->getConstraintNotificationMap().empty());
// update solver
ceres_manager_ptr->update();
// check constraint
ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
TEST(CeresManager, DoubleRemoveConstraint)
@@ -349,10 +421,6 @@ TEST(CeresManager, DoubleRemoveConstraint)
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create State block
- Vector2s state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
// Create (and add) constraint point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
@@ -363,13 +431,13 @@ TEST(CeresManager, DoubleRemoveConstraint)
ceres_manager_ptr->update();
// remove constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
// update solver
ceres_manager_ptr->update();
// remove constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
ASSERT_DEATH({
// update solver
@@ -377,8 +445,191 @@ TEST(CeresManager, DoubleRemoveConstraint)
// check constraint
ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+TEST(CeresManager, AddStateBlockLocalParam)
+{
+ ProblemPtr P = Problem::create("PO 2D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+ // Create State block
+ Vector1s state; state << 1;
+ StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+ // Local param
+ LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
+ sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+
+ // add stateblock
+ P->addStateBlock(sb_ptr);
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check stateblock
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
}
+TEST(CeresManager, RemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO 2D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+ // Create State block
+ Vector1s state; state << 1;
+ StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+ // Local param
+ LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
+ sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+
+ // add stateblock
+ P->addStateBlock(sb_ptr);
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check stateblock
+ ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+TEST(CeresManager, AddLocalParam)
+{
+ ProblemPtr P = Problem::create("PO 2D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+ // Create State block
+ Vector1s state; state << 1;
+ StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+ // add stateblock
+ P->addStateBlock(sb_ptr);
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check stateblock
+ ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
+
+ // Local param
+ LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
+ sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check stateblock
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+TEST(CeresManager, ConstraintsRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO 3D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+ // Create (and add) 2 constraints quaternion
+ FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+ FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+ ConstraintQuaternionAbsolutePtr c1 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+ ConstraintQuaternionAbsolutePtr c2 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check local param
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+
+ // check constraint
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+
+ // remove local param
+ F->getOPtr()->removeLocalParametrization();
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check local param
+ ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+
+ // check constraint
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+TEST(CeresManager, ConstraintsUpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO 3D");
+ CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+ // Create (and add) 2 constraints quaternion
+ FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+ CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+ FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+ ConstraintQuaternionAbsolutePtr c1 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+ ConstraintQuaternionAbsolutePtr c2 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check local param
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+
+ // check constraint
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+
+ // remove local param
+ LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
+ F->getOPtr()->setLocalParametrizationPtr(local_param_ptr);
+
+ // update solver
+ ceres_manager_ptr->update();
+
+ // check local param
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),local_param_ptr));
+
+ // check constraint
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+
+ // run ceres manager check
+ ceres_manager_ptr->check();
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index 9ab18d070e75c5f9e4a01f7434dc192ce7c84595..7ae62578b7d99441e56539a94830696e6489253c 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -214,12 +214,12 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
ASSERT_EQ(P->getStateBlockList().size(), (unsigned int) 2);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
ASSERT_EQ(P->getStateBlockList().size(), (unsigned int) (2 + 3));
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) (2 + 3));
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
@@ -233,7 +233,7 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int)(2 + 3 + 2));
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
// P->print(4,1,1,1);
@@ -241,7 +241,7 @@ TEST(Problem, StateBlocks)
// change some SB properties
St->unfixExtrinsics();
ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getNotifiedStateBlockList().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
// P->print(4,1,1,1);
}
diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp
index f2aee334e543ab02507c302d44a010555a70e4f1..effdfc1401c6ece1822651f287dfb7d4ffb1bbb3 100644
--- a/src/test/gtest_solver_manager.cpp
+++ b/src/test/gtest_solver_manager.cpp
@@ -14,6 +14,8 @@
#include "../capture_void.h"
#include "../constraint_pose_2D.h"
#include "../solver/solver_manager.h"
+#include "../local_parametrization_base.h"
+#include "../local_parametrization_angle.h"
#include <iostream>
@@ -26,6 +28,7 @@ class SolverManagerWrapper : public SolverManager
public:
std::list<ConstraintBasePtr> constraints_;
std::map<StateBlockPtr,bool> state_block_fixed_;
+ std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
SolverManagerWrapper(const ProblemPtr& wolf_problem) :
SolverManager(wolf_problem)
@@ -47,6 +50,16 @@ class SolverManagerWrapper : public SolverManager
return std::find(constraints_.begin(), constraints_.end(), ctr_ptr) != constraints_.end();
};
+ bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
+ };
+
+ bool hasLocalParametrization(const StateBlockPtr& st) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end();
+ };
+
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
virtual void computeCovariances(const StateBlockList& st_list){};
@@ -64,15 +77,24 @@ class SolverManagerWrapper : public SolverManager
virtual void addStateBlock(const StateBlockPtr& state_ptr)
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
};
virtual void removeStateBlock(const StateBlockPtr& state_ptr)
{
state_block_fixed_.erase(state_ptr);
+ state_block_local_param_.erase(state_ptr);
};
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
};
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+ {
+ if (state_ptr->getLocalParametrizationPtr() == nullptr)
+ state_block_local_param_.erase(state_ptr);
+ else
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
+ };
};
TEST(SolverManager, Create)
@@ -140,18 +162,38 @@ TEST(SolverManager, UpdateStateBlock)
// add stateblock
P->addStateBlock(sb_ptr);
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// update solver
solver_manager_ptr->update();
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// check stateblock unfixed
ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
// Fix frame
sb_ptr->fix();
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_TRUE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// update solver manager
solver_manager_ptr->update();
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// check stateblock fixed
ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
}
@@ -168,15 +210,120 @@ TEST(SolverManager, AddUpdateStateBlock)
// add stateblock
P->addStateBlock(sb_ptr);
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// Fix state block
sb_ptr->fix();
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_TRUE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// update solver manager
solver_manager_ptr->update();
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+}
+
+TEST(SolverManager, AddUpdateLocalParamStateBlock)
+{
+ 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);
+
+ // add stateblock
+ P->addStateBlock(sb_ptr);
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ sb_ptr->setLocalParametrizationPtr(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_TRUE(sb_ptr->fixUpdated());
+ ASSERT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_manager_ptr->update();
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
// check stateblock fixed
ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+}
+
+TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+{
+ 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);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ sb_ptr->setLocalParametrizationPtr(local_ptr);
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_TRUE(sb_ptr->localParamUpdated());
+
+ // add stateblock
+ P->addStateBlock(sb_ptr);
+
+ // update solver manager
+ solver_manager_ptr->update();
+
+ // check stateblock localparam
+ ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ ASSERT_FALSE(sb_ptr->stateUpdated());
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_manager_ptr->update();
+
+ // check stateblock localparam
+ ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
}
TEST(SolverManager, RemoveStateBlock)
@@ -195,7 +342,7 @@ TEST(SolverManager, RemoveStateBlock)
solver_manager_ptr->update();
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
@@ -217,7 +364,7 @@ TEST(SolverManager, AddRemoveStateBlock)
P->addStateBlock(sb_ptr);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
@@ -239,7 +386,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
P->addStateBlock(sb_ptr);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
@@ -258,13 +405,13 @@ TEST(SolverManager, DoubleRemoveStateBlock)
P->addStateBlock(sb_ptr);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
// update solver manager
solver_manager_ptr->update();
@@ -286,65 +433,46 @@ TEST(SolverManager, AddUpdatedStateBlock)
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);
+ // Check flags have been set true
+ ASSERT_TRUE(sb_ptr->fixUpdated());
+ ASSERT_TRUE(sb_ptr->stateUpdated());
// == 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);
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
+ ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD);
// == Insert OTHER notifications ==
- // Set State
+ // Set State --> FLAG
state_2 = 2*state;
sb_ptr->setState(state_2);
- // Fix
+ // Fix --> FLAG
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);
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb)
- // == REMOVE should clear all previous notifications in the stack because there's an ADD ==
+ // == REMOVE should clear the previous notification (ADD) in the stack ==
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
-
- P_notif_sb = P->getNotifiedStateBlockList();
- ASSERT_EQ(P_notif_sb.size(),1);
+ P->removeStateBlock(sb_ptr);
- sb_notif = P_notif_sb.front()->getNotifications();
- ASSERT_EQ(sb_notif.size(),0); // ADD + REMOVE = EMPTY
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY
// == UPDATES + REMOVE should clear the list of notifications ==
+ // add state_block
+ P->addStateBlock(sb_ptr);
+
+ // update solver
+ solver_manager_ptr->update();
+
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty
+
// Fix
sb_ptr->fix();
@@ -353,11 +481,10 @@ TEST(SolverManager, AddUpdatedStateBlock)
sb_ptr->setState(state_2);
// remove state_block
- P->removeStateBlockPtr(sb_ptr);
+ P->removeStateBlock(sb_ptr);
- sb_notif = P_notif_sb.front()->getNotifications();
- ASSERT_EQ(sb_notif.size(),1);
- ASSERT_EQ(sb_notif.front(),StateBlock::Notification::REMOVE);
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
+ ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
}
TEST(SolverManager, AddConstraint)
@@ -401,7 +528,7 @@ TEST(SolverManager, RemoveConstraint)
solver_manager_ptr->update();
// add constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
// update solver
solver_manager_ptr->update();
@@ -425,12 +552,12 @@ TEST(SolverManager, AddRemoveConstraint)
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
- ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c);
+ ASSERT_TRUE(P->getConstraintNotificationMap().begin()->first == c);
// add constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
- ASSERT_TRUE(P->getConstraintNotificationList().empty());
+ ASSERT_TRUE(P->getConstraintNotificationMap().empty());
// update solver
solver_manager_ptr->update();
@@ -458,13 +585,13 @@ TEST(SolverManager, DoubleRemoveConstraint)
solver_manager_ptr->update();
// remove constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
// update solver
solver_manager_ptr->update();
// remove constraint
- P->removeConstraintPtr(c);
+ P->removeConstraint(c);
// update solver
solver_manager_ptr->update();
diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index 17e0667475716d16b694ee8c2b3d8139e91035af..92c8480399d9bdd77038672f48465e2d93294f5c 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -32,42 +32,25 @@ struct DummyNotificationProcessor
FAIL() << "problem_ is nullptr !";
}
- StateBlockList& states = problem_->getNotifiedStateBlockList();
-
- for (StateBlockPtr& state : states)
+ auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin();
+ while (sb_noti_pair != problem_->getStateBlockNotificationMap().end())
{
- const auto notifications = state->consumeNotifications();
-
- for (const auto notif : notifications)
- {
- switch (notif)
+ switch (sb_noti_pair->second)
{
- case StateBlock::Notification::ADD:
- {
- break;
- }
- case StateBlock::Notification::UPDATE_STATE:
+ case ADD:
{
break;
}
- case StateBlock::Notification::UPDATE_FIX:
- {
- break;
- }
- case StateBlock::Notification::REMOVE:
+ case REMOVE:
{
break;
}
default:
- throw std::runtime_error("SolverManager::update: State Block notification "
- "must be ADD, STATE_UPDATE, FIX_UPDATE, REMOVE or ENABLED.");
+ throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
}
- }
-
- ASSERT_FALSE(state->hasNotifications());
+ sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair);
}
-
- states.clear();
+ ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty());
}
ProblemPtr problem_;
@@ -131,45 +114,55 @@ TEST(TrajectoryBase, Add_Remove_Frame)
FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+ std::cout << __LINE__ << std::endl;
+
// add frames and keyframes
T->addFrame(f1); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ std::cout << __LINE__ << std::endl;
T->addFrame(f2); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 4);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+ std::cout << __LINE__ << std::endl;
T->addFrame(f3); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 4);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+ std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ std::cout << __LINE__ << std::endl;
N.update();
+ std::cout << __LINE__ << std::endl;
// remove frames and keyframes
f2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
+ std::cout << __LINE__ << std::endl;
f3->remove(); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
@@ -177,11 +170,13 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 4);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+ std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_EQ(P->getNotifiedStateBlockList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0);
+ std::cout << __LINE__ << std::endl;
}
TEST(TrajectoryBase, KeyFramesAreSorted)