diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 70fee2c15f074e571fd273d7cfccfbdc21c20552..2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -79,6 +79,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
public:
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
// State blocks
const std::string& getStructure() const;
@@ -183,6 +185,7 @@ inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
return constrained_by_list_;
}
+
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 9f548fd325de45d8901f432975bf23563938c988..64a4741c518ecd0f75bf029aa4e3267b1e3f8c4e 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -195,7 +195,11 @@ struct MatrixSizeCheck
typedef std::list<ClassName##Ptr> ClassName##PtrList; \
typedef ClassName##PtrList::iterator ClassName##Iter; \
typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 6f215396071b757b980faac31be8d0250716ea71..cd1439f96e437dc39dde137dd1014f37abee10db 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -40,19 +40,19 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
static unsigned int factor_id_count_;
protected:
unsigned int factor_id_;
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer
- CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list
virtual void setProblem(ProblemPtr) final;
public:
@@ -72,6 +72,17 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
+ FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+
+
virtual ~FactorBase() = default;
virtual void remove(bool viral_remove_empty_parent=false);
@@ -133,6 +144,14 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
CaptureBasePtr getCapture() const;
+ /** \brief Returns a pointer to its frame
+ **/
+ FrameBasePtr getFrame() const;
+
+ /** \brief Returns a pointer to its capture's sensor
+ **/
+ SensorBasePtr getSensor() const;
+
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
@@ -149,21 +168,32 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
bool getApplyLossFunction() const;
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
+ FrameBasePtr getFrameOther() const;
- /** \brief Returns a pointer to the capture constrained to
+ /** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
+ CaptureBasePtr getCaptureOther() const;
- /** \brief Returns a pointer to the feature constrained to
+ /** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
+ FeatureBasePtr getFeatureOther() const;
- /** \brief Returns a pointer to the landmark constrained to
+ /** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
+ LandmarkBasePtr getLandmarkOther() const;
+
+ // get pointer lists to other nodes
+ FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
+
+ bool hasFrameOther(const FrameBasePtr& _frm_other) const;
+ bool hasCaptureOther(const CaptureBasePtr& _cap_other) const;
+ bool hasFeatureOther(const FeatureBasePtr& _ftr_other) const;
+ bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other) const;
public:
/**
@@ -228,6 +258,39 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
+inline FrameBasePtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeatureOther() const
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
+
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 2338f699de73d38c74ce13509cdc0b70a1a82130..d936a99d6c1d64f29fda2c5aeca1806e0065c4d1 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -140,10 +140,12 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
inline Eigen::VectorXd FactorDiffDrive::residual()
{
VectorXd residual(3);
- operator ()(getFrameOther()->getP()->getState().data(), getFrameOther()->getO()->getState().data(),
- getCapture()->getFrame()->getP()->getState().data(),
- getCapture()->getFrame()->getO()->getState().data(),
- getCaptureOther()->getSensorIntrinsic()->getState().data(), residual.data());
+ operator ()(getFrameOther() ->getP() ->getState() .data(),
+ getFrameOther() ->getO() ->getState() .data(),
+ getFrame() ->getP() ->getState() .data(),
+ getFrame() ->getO() ->getState() .data(),
+ getCaptureOther() ->getSensorIntrinsic() ->getState() .data(),
+ residual.data());
return residual;
}
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 7203425913de8bb5a3ff5a0fb267b4aef98f9fd5..a2ba3bc60f5d0db16d6c486099ac5e3e9b3116ec 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -95,6 +95,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
+
// all factors
void getFactorList(FactorBasePtrList & _fac_list) const;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 3745de04806c78f65463c39da49bcf9ba42cf0f8..eea8873b3e804743f4cd224bffd6442d4c17febf 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -126,6 +126,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
void getFactorList(FactorBasePtrList& _fac_list) const;
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr& _factor) const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 2f80a3d6fbe26d171126a82d280e65a3acaf4349..254e03e480f8471029c38c8f59626b230dc962f9 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -71,6 +71,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
MapBasePtr getMap() const;
void link(MapBasePtr);
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 97c570cb8550d5fb9aa6a2bf076e7e9674ab75c5..07c6d757bc09125c4a4e8a55aefd6d37b480c7d4 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -48,10 +48,13 @@ class HasStateBlocks
virtual unsigned int removeStateBlock(const char _sb_type);
bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
+ bool hasStateBlock(const StateBlockPtr& _sb) const;
StateBlockPtr getStateBlock(const std::string& _sb_type) const;
StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
+ bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
+ std::unordered_map<std::string, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
@@ -283,7 +286,49 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
return size;
}
+//<<<<<<< HEAD
+inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
+{
+ const auto& it = this->find(_sb);
+
+ return it != state_block_map_.end();
+}
+
+inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = "";
+ return false;
+ }
+}
+
+//inline unsigned int HasStateBlocks::getLocalSize() const
+//=======
inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
+//>>>>>>> devel
{
if (_sub_structure == ""){
_sub_structure = structure_;
@@ -302,4 +347,5 @@ inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) con
}
} // namespace wolf
+
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
index db3ab2dc9dd6e165c3541544bf7770022568f2fe..844b4d8ea60821ae18b6a9912d343de703163814 100644
--- a/include/core/utils/check_log.hpp
+++ b/include/core/utils/check_log.hpp
@@ -2,31 +2,42 @@
#define CHECK_LOG_HPP
#include <iostream>
#include <string>
+#include <sstream>
-class CheckLog {
+namespace wolf
+{
+class CheckLog
+{
+ public:
+ bool is_consistent_;
+ std::string explanation_;
-public:
-
- bool is_consistent_;
- std::string explanation_;
-
- CheckLog() {
- is_consistent_ = true;
- explanation_ = "";
- }
- CheckLog(bool consistent, std::string explanation) {
- is_consistent_ = consistent;
- if (not consistent)
- explanation_ = explanation;
- else
- explanation_ = "";
- }
- ~CheckLog(){};
- void compose(CheckLog l) {
-
- CheckLog result_log;
- is_consistent_ = is_consistent_ and l.is_consistent_;
- explanation_ = explanation_ + l.explanation_;
- }
+ CheckLog()
+ {
+ is_consistent_ = true;
+ explanation_ = "";
+ }
+ CheckLog(bool _consistent, std::string _explanation)
+ {
+ is_consistent_ = _consistent;
+ if (not _consistent)
+ explanation_ = _explanation;
+ else
+ explanation_ = "";
+ }
+ ~CheckLog(){};
+ void compose(CheckLog l)
+ {
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+ }
+ void assertTrue(bool _condition, std::stringstream& _stream)
+ {
+ auto cl = CheckLog(_condition, _stream.str());
+ this->compose(cl);
+ // Clear inconsistency_explanation
+ std::stringstream().swap(_stream);
+ }
};
+} // namespace wolf
#endif
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 54e5d2a3cdedb301a7580e691cad9543df3c24c2..d07b9b055d8197593dbdde99d05f6a4eddd46bfe 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -130,6 +130,22 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
+
const std::string& CaptureBase::getStructure() const
{
if (getSensor())
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 46b244d9e83f9c01f4fca0bf84af35d81d36d53f..72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -19,15 +19,52 @@ FactorBase::FactorBase(const std::string& _tp,
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(_frame_other_ptr),
- capture_other_ptr_(_capture_other_ptr),
- feature_other_ptr_(_feature_other_ptr),
- landmark_other_ptr_(_landmark_other_ptr),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
processor_ptr_(_processor_ptr)
{
-// std::cout << "constructed +c" << id() << std::endl;
+ if (_frame_other_ptr)
+ frame_other_list_.push_back(_frame_other_ptr);
+ if (_capture_other_ptr)
+ capture_other_list_.push_back(_capture_other_ptr);
+ if (_feature_other_ptr)
+ feature_other_list_.push_back(_feature_other_ptr);
+ if (_landmark_other_ptr)
+ landmark_other_list_.push_back(_landmark_other_ptr);
}
+FactorBase::FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
+ feature_ptr_(),
+ factor_id_(++factor_id_count_),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
+ processor_ptr_(_processor_ptr)
+{
+ for (auto& Fo : _frame_other_list)
+ frame_other_list_.push_back(Fo);
+ for (auto& Co : _capture_other_list)
+ capture_other_list_.push_back(Co);
+ for (auto& fo : _feature_other_list)
+ feature_other_list_.push_back(fo);
+ for (auto& Lo : landmark_other_list_)
+ landmark_other_list_.push_back(Lo);
+}
+
+
void FactorBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
@@ -46,36 +83,48 @@ void FactorBase::remove(bool viral_remove_empty_parent)
getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
- FrameBasePtr frm_o = frame_other_ptr_.lock();
- if (frm_o)
+ for (auto& frm_ow : frame_other_list_)
{
- frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove(viral_remove_empty_parent);
+ auto frm_o = frm_ow.lock();
+ if (frm_o)
+ {
+ frm_o->removeConstrainedBy(this_fac);
+ if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ frm_o->remove(viral_remove_empty_parent);
+ }
}
- CaptureBasePtr cap_o = capture_other_ptr_.lock();
- if (cap_o)
+ for (auto& cap_ow : capture_other_list_)
{
- cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove(viral_remove_empty_parent);
+ auto cap_o = cap_ow.lock();
+ if (cap_o)
+ {
+ cap_o->removeConstrainedBy(this_fac);
+ if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ cap_o->remove(viral_remove_empty_parent);
+ }
}
- FeatureBasePtr ftr_o = feature_other_ptr_.lock();
- if (ftr_o)
+ for (auto& ftr_ow : feature_other_list_)
{
- ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove(viral_remove_empty_parent);
+ auto ftr_o = ftr_ow.lock();
+ if (ftr_o)
+ {
+ ftr_o->removeConstrainedBy(this_fac);
+ if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ ftr_o->remove(viral_remove_empty_parent);
+ }
}
- LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
- if (lmk_o)
+ for (auto& lmk_ow : landmark_other_list_)
{
- lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove(viral_remove_empty_parent);
+ auto lmk_o = lmk_ow.lock();
+ if (lmk_o)
+ {
+ lmk_o->removeConstrainedBy(this_fac);
+ if (lmk_o->getConstrainedByList().empty())
+ lmk_o->remove(viral_remove_empty_parent);
+ }
}
// std::cout << "Removed c" << id() << std::endl;
@@ -106,6 +155,18 @@ CaptureBasePtr FactorBase::getCapture() const
return getFeature()->getCapture();
}
+FrameBasePtr FactorBase::getFrame() const
+{
+ assert(getCapture() != nullptr && "calling getFrame before linking with a capture");
+ return getCapture()->getFrame();
+}
+
+SensorBasePtr FactorBase::getSensor() const
+{
+ assert(getCapture() != nullptr && "calling getSensor before linking with a capture");
+ return getCapture()->getSensor();
+}
+
void FactorBase::setStatus(FactorStatus _status)
{
if (getProblem() == nullptr)
@@ -120,6 +181,66 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
+bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other) const
+{
+ FrameBaseWConstIter frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
+ [_frm_other](const FrameBaseWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ }
+ );
+ if (frm_it != frame_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other) const
+{
+ CaptureBaseWConstIter cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
+ [_cap_other](const CaptureBaseWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ }
+ );
+ if (cap_it != capture_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other) const
+{
+ FeatureBaseWConstIter ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
+ [_ftr_other](const FeatureBaseWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ }
+ );
+ if (ftr_it != feature_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
+{
+ LandmarkBaseWConstIter lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
+ [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ }
+ );
+ if (lmk_it != landmark_other_list_.end())
+ return true;
+
+ return false;
+}
+
//void FactorBase::setApplyLossFunction(const bool _apply)
//{
// apply_loss_function_ = _apply;
@@ -146,14 +267,26 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
this->setProblem(_ftr_ptr->getProblem());
// constrained by
- auto frame_other = this->frame_other_ptr_.lock();
- if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
- auto capture_other = this->capture_other_ptr_.lock();
- if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
- auto feature_other = this->feature_other_ptr_.lock();
- if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
- auto landmark_other = this->landmark_other_ptr_.lock();
- if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ for (auto& frm_ow : frame_other_list_)
+ {
+ auto frame_other = frm_ow.lock();
+ if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& cap_ow : capture_other_list_)
+ {
+ auto capture_other = cap_ow.lock();
+ if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& ftr_ow : feature_other_list_)
+ {
+ auto feature_other = ftr_ow.lock();
+ if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& lmk_ow : landmark_other_list_)
+ {
+ auto landmark_other = lmk_ow.lock();
+ if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ }
}
void FactorBase::setProblem(ProblemPtr _problem)
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index df5d1f1e7aa0f214adab2e0f74dc496623e7cf48..8dc3801efa5f93e5bd0df593442e4860cb75135e 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -92,6 +92,21 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
const FactorBasePtrList& FeatureBase::getFactorList() const
{
return factor_list_;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 42a11b3782a5a669264471bcae49571ec1322840..c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -314,6 +314,21 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 7c9b8b664573bd296902ad420c1e5a161605e635..bc57f27294e6a0b05d67cbcf44725dd2b819f63b 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -138,6 +138,21 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index e79de9572f545e85142450497f3d4780621adc7b..1d72f5b8fff0b187d192e575eb140a2e442f101e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -925,227 +925,226 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric, bool state_blocks) const
+void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
{
- stream << std::endl;
- stream << "P: wolf tree status ---------------------" << std::endl;
- stream << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
- if (depth >= 1)
+ _stream << std::endl;
+ _stream << "P: wolf tree status ---------------------" << std::endl;
+ _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
+ if (_depth >= 1)
{
// Sensors =======================================================================================
for (auto S : getHardware()->getSensorList())
{
- stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- stream << " -- " << S->getProcessorList().size() << "p";
- stream << std::endl;
- if (metric && state_blocks)
+ _stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << S->getProcessorList().size() << "p";
+ _stream << std::endl;
+ if (_metric && _state_blocks)
{
- stream << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- stream << std::endl;
+ _stream << std::endl;
}
- else if (metric)
+ else if (_metric)
{
- stream << " ( ";
+ _stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << sb->getState().transpose() << " ";
+ _stream << sb->getState().transpose() << " ";
}
- stream << ")" << std::endl;
+ _stream << ")" << std::endl;
}
- else if (state_blocks)
+ else if (_state_blocks)
{
- stream << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
- stream << std::endl;
+ _stream << std::endl;
}
- if (depth >= 2)
+ if (_depth >= 2)
{
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
{
- stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
+ _stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- stream << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ _stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
<< pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
- stream << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
- stream << " i: C" << pm->getIncoming()->id() << std::endl;
+ _stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
}
else
{
- stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
+ _stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
- stream << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
- stream << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
- stream << " i: C" << pt->getIncoming()->id() << std::endl;
+ _stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
- stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
- if (depth >= 1)
+ _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
+ if (_depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((_depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ if (_constr_by)
{
- stream << " <-- ";
+ _stream << " <-- ";
for (auto cby : F->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ _stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp();
- stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- stream << std::endl;
+ _stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ _stream << std::endl;
}
- if (state_blocks)
+ if (_state_blocks)
{
- stream << " sb:";
+ _stream << " sb:";
for (const auto& sb : F->getStateBlockVec())
{
- stream << " " << (sb->isFixed() ? "Fix" : "Est");
+ _stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- stream << std::endl;
+ _stream << std::endl;
}
- // if (state_blocks)
- // {
- // cout << " sb:";
- // for (const auto& sb : F->getStateBlockVec())
- // {
- // cout << " " << (sb->isFixed() ? "Fix" : "Est");
- // }
- // cout << endl;
- // }
- if (depth >= 2)
+ if (_depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- stream << " -> Sen" << C->getSensor()->id();
+ _stream << " -> Sen" << C->getSensor()->id();
}
else
- stream << " -> S-";
+ _stream << " -> Sen-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
- stream << " -> OCap" << OC->id();
+ _stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- stream << " ; OFrm" << OF->id();
+ _stream << " ; OFrm" << OF->id();
}
}
- stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
+ _stream << ((_depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ if (_constr_by)
{
- stream << " <-- ";
+ _stream << " <-- ";
for (auto cby : C->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
+ _stream << std::endl;
- if (state_blocks)
+ if (_state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
- stream << " sb: ";
- stream << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- stream << std::endl;
+ _stream << " sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
- if ( metric && ! CM->getBuffer().get().empty())
+ _stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
+ if ( _metric && ! CM->getBuffer().get().empty())
{
- stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
+ _stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
- stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
- stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
- stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
- stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
+ _stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
+ _stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
+ _stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
+ _stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
}
}
}
- if (depth >= 3)
+ if (_depth >= 3)
{
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
+ _stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((_depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ if (_constr_by)
{
- stream << " <--\t";
+ _stream << " <--\t";
for (auto cby : f->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
- stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ _stream << std::endl;
+ if (_metric)
+ _stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
<< " )" << std::endl;
- if (depth >= 4)
+ if (_depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- stream << " Fac" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- stream << " Abs";
- if (c->getFrameOther() != nullptr)
- stream << " Frm" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- stream << " Cap" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- stream << " Fac" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- stream << " Lmk" << c->getLandmarkOther()->id();
- stream << std::endl;
+ _stream << " Fac" << c->id() << " " << c->getType() << " -->";
+ if ( c->getFrameOtherList() .empty()
+ && c->getCaptureOtherList() .empty()
+ && c->getFeatureOtherList() .empty()
+ && c->getLandmarkOtherList().empty())
+ _stream << " Abs";
+
+ for (const auto& Fow : c->getFrameOtherList())
+ if (!Fow.expired())
+ _stream << " Frm" << Fow.lock()->id();
+ for (const auto& Cow : c->getCaptureOtherList())
+ if (!Cow.expired())
+ _stream << " Cap" << Cow.lock()->id();
+ for (const auto& fow : c->getFeatureOtherList())
+ if (!fow.expired())
+ _stream << " Ftr" << fow.lock()->id();
+ for (const auto& Low : c->getLandmarkOtherList())
+ if (!Low.expired())
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
} // for c
}
} // for f
@@ -1154,236 +1153,208 @@ void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric
}
} // for F
}
- stream << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
- if (depth >= 1)
+ _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
+ if (_depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
- stream << " Lmk" << L->id() << " " << L->getType();
- if (constr_by)
+ _stream << " Lmk" << L->id() << " " << L->getType();
+ if (_constr_by)
{
- stream << "\t<-- ";
+ _stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- stream << (L->isFixed() ? " Fix" : " Est");
- stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- stream << std::endl;
+ _stream << (L->isFixed() ? " Fix" : " Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ _stream << std::endl;
}
- if (state_blocks)
+ if (_state_blocks)
{
- stream << " sb:";
+ _stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
- stream << (sb->isFixed() ? " Fix" : " Est");
+ _stream << (sb->isFixed() ? " Fix" : " Est");
}
- stream << std::endl;
+ _stream << std::endl;
}
} // for L
}
- stream << "-----------------------------------------" << std::endl;
- stream << std::endl;
+ _stream << "-----------------------------------------" << std::endl;
+ _stream << std::endl;
}
-bool Problem::check(bool verbose, std::ostream& stream) const
+bool Problem::check(bool _verbose, std::ostream& _stream) const
{
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- if (verbose) stream << std::endl;
- if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
- auto P_raw = this;
- if (verbose)
+ if (_verbose) _stream << std::endl;
+ if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
+ auto P = shared_from_this();
+ if (_verbose)
{
- stream << "Prb @ " << P_raw << std::endl;
+ _stream << "Prb @ " << P.get() << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose)
+ if (_verbose)
{
- stream << "Hrw @ " << H.get() << std::endl;
+ _stream << "Hrw @ " << H.get() << std::endl;
}
- // check pointer to Problem
- // is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+ // check pointer to Problem
std::stringstream inconsistency_explanation;
inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((H->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((H->getProblem() == P), inconsistency_explanation);
+
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
- stream << " -> Prb @ " << S->getProblem().get() << std::endl;
- stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
+ _stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
+ _stream << " -> Prb @ " << S->getProblem().get() << std::endl;
+ _stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
for (auto pair: S->getStateBlockMap())
{
auto sb = pair.second;
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
+
// check problem and hardware pointers
- // is_consistent = is_consistent && (S->getProblem().get() == P_raw);
inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((S->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((S->getProblem() == P), inconsistency_explanation);
+
- // is_consistent = is_consistent && (S->getHardware() == H);
inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
<< " but hardware pointer is " << H << "\n";
- log.compose(CheckLog((S->getHardware() == H), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((S->getHardware() == H), inconsistency_explanation);
+
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
- stream << " -> Prb @ " << p->getProblem().get() << std::endl;
- stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
+ _stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
+ _stream << " -> Prb @ " << p->getProblem().get() << std::endl;
+ _stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
}
- // check problem and sensor pointers
-
- // is_consistent = is_consistent && (p->getProblem().get() == P_raw);
+ // check problem and sensor pointers
inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (p->getSensor() == S);
inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
- << " but sensor pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but sensor pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
+
}
}
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose)
+ if (_verbose)
{
- stream << "Trj @ " << T.get() << std::endl;
+ _stream << "Trj @ " << T.get() << std::endl;
}
- // check pointer to Problem
- // is_consistent = is_consistent && (T->getProblem().get() == P_raw);
-
+ // check pointer to Problem
inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((T->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((T->getProblem() == P), inconsistency_explanation);
+
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose) {
- stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
+ if (_verbose) {
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
<< F->id() << " @ " << F.get() << std::endl;
- stream << " -> Prb @ " << F->getProblem().get() << std::endl;
- stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
+ _stream << " -> Prb @ " << F->getProblem().get() << std::endl;
+ _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
}
for (const auto &pair: F->getStateBlockMap()) {
+
auto sb = pair.second;
- inconsistency_explanation << "Frame's " << F.get()
+
+ // check for valid state block
+ inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
<< " has State block pointer " << sb.get()
- << " null! \n";
- log.compose(
- CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- if (verbose) {
- stream << " "<< pair.first << " sb @ " << sb.get();
+ << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose) {
+ _stream << " "<< pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
- // check problem and trajectory pointers
- // is_consistent = is_consistent && (F->getProblem().get() == P_raw);
-
+ // check problem pointer
inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((F->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- // is_consistent = is_consistent && (F->getTrajectory() == T);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((F->getProblem() == P), inconsistency_explanation);
+ // check trajectory pointer
inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
- << " but trajectory pointer is" << T << "\n";
- log.compose(CheckLog((F->getTrajectory() == T), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but trajectory pointer is" << T << "\n";
+ log.assertTrue((F->getTrajectory() == T), inconsistency_explanation);
+
+ // check constrained_by
for (auto cby : F->getConstrainedByList())
{
- for (auto sb : cby->getStateBlockPtrVector()) {
- inconsistency_explanation
- << "Factor " << cby.get() << " constraining " << F.get()
- << " has State block pointer " << sb.get() << " null! \n";
- log.compose(
- CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- }
- if (verbose) {
- stream << " <- c" << cby->id() << " -> Frm"
- << cby->getFrameOther()->id() << std::endl;
- }
- // check constrained_by pointer to this frame
- // is_consistent = is_consistent && (cby->getFrameOther() == F);
+ if (_verbose)
+ {
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ _stream << " F" << Fow.lock()->id() << std::endl;
+ }
- inconsistency_explanation << "constrained-by frame pointer is " << cby->getFrameOther()
- << " but frame pointer is" << F << "\n";
- log.compose(CheckLog((cby->getFrameOther() == F), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check constrained_by pointer to this frame
+ inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose) {
- stream << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
}
@@ -1391,361 +1362,383 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
- if (C->getSensor()) stream << C->getSensor()->id();
- else stream << "-";
- stream << std::endl;
- stream << " -> Prb @ " << C->getProblem().get() << std::endl;
- stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
+ _stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
+ if (C->getSensor()) _stream << C->getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << " -> Prb @ " << C->getProblem().get() << std::endl;
+ _stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
}
- for (auto pair: C->getStateBlockMap())
+ for (auto pair: C->getStateBlockMap())
+ {
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose)
{
- auto sb = pair.second;
- if (verbose)
- {
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- stream << " (lp @ " << lp.get() << ")";
- }
- stream << std::endl;
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
}
+ _stream << std::endl;
}
+ }
// check problem and frame pointers
- // is_consistent = is_consistent && (C->getProblem().get() == P_raw);
-
inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((C->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((C->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (C->getFrame() == F);
inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
<< " but frame pointer is" << F << "\n";
- log.compose(CheckLog((C->getFrame() == F), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((C->getFrame() == F), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (const auto& cby : C->getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ _stream << " C" << Cow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this capture
+ inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasCaptureOther(C)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose)
+ {
+ _stream << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ }
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
- stream << " -> Prb @ " << f->getProblem().get() << std::endl;
- stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
+ _stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
+ _stream << " -> Prb @ " << f->getProblem().get() << std::endl;
+ _stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
<< std::endl;
}
- // check problem and capture pointers
- // is_consistent = is_consistent && (f->getProblem().get() == P_raw);
+ // check problem and capture pointers
inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((f->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((f->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (f->getCapture() == C);
inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
<< " but capture pointer is" << C << "\n";
- log.compose(CheckLog((f->getCapture() == C), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((f->getCapture() == C), inconsistency_explanation);
+
+ // check contrained_by
for (auto cby : f->getConstrainedByList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " <- c" << cby->id() << " -> Ftr" << cby->getFeatureOther()->id() << std::endl;
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ _stream << " f" << fow.lock()->id();
+ _stream << std::endl;
}
+
// check constrained_by pointer to this feature
- // is_consistent = is_consistent && (cby->getFeatureOther() == f);
+ inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFeatureOther(f)), inconsistency_explanation);
- inconsistency_explanation << "constrained by Feature pointer is " << cby->getFeatureOther()
- << " but feature pointer is" << f << "\n";
- log.compose(CheckLog((cby->getFeatureOther() == f), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- if (verbose)
- stream << " Fac" << c->id() << " @ " << c.get();
+ if (_verbose)
+ _stream << " Fac" << c->id() << " @ " << c.get();
- auto Fo = c->getFrameOther();
- auto Co = c->getCaptureOther();
- auto fo = c->getFeatureOther();
- auto Lo = c->getLandmarkOther();
-
- if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
+ if ( c->getFrameOtherList() .empty()
+ && c->getCaptureOtherList() .empty()
+ && c->getFeatureOtherList() .empty()
+ && c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
{
- if (verbose)
- stream << " --> Abs." << std::endl;
+ if (_verbose)
+ _stream << " --> Abs.";
}
// find constrained_by pointer in constrained frame
- if ( Fo ) // case FRAME:
+ for (const auto& Fow : c->getFrameOtherList())
{
- if (verbose)
- stream << " ( --> Frm" << Fo->id() << " <- ";
- bool found = false;
- for (auto cby : Fo->getConstrainedByList())
+ if (!Fow.expired())
{
- if (verbose)
- stream << " Fac" << cby->id();
- found = found || (c == cby);
- }
- if (verbose)
- stream << ")";
- // check constrained_by pointer in constrained frame
- // is_consistent = is_consistent && found;
+ const auto& Fo = Fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> F" << Fo->id() << " <- ";
+ for (auto cby : Fo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
- inconsistency_explanation << "The constrained Feature " << Fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check constrained_by pointer in constrained frame
+ bool found = Fo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
}
+ if (_verbose && !c->getFrameOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained capture
- if ( Co ) // case CAPTURE:
+ for (const auto& Cow : c->getCaptureOtherList())
{
- if (verbose)
- stream << " ( --> Cap" << Co->id() << " <- ";
- bool found = false;
- for (auto cby : Co->getConstrainedByList())
+ if (!Cow.expired())
{
- if (verbose)
- stream << " Fac" << cby->id();
- found = found || (c == cby);
- }
- if (verbose)
- stream << ")";
- // check constrained_by pointer in constrained frame
- // is_consistent = is_consistent && found;
+ const auto& Co = Cow.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> C" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
- inconsistency_explanation << "The constrained capture " << Co
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check constrained_by pointer in constrained frame
+ bool found = Co->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
}
+ if (_verbose && !c->getCaptureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained feature
- if ( fo ) // case FEATURE:
+ for (const auto& fow : c->getFeatureOtherList())
{
- if (verbose)
- stream << " ( --> Ftr" << fo->id() << " <- ";
- bool found = false;
- for (auto cby : fo->getConstrainedByList())
+ if (!fow.expired())
{
- if (verbose)
- stream << " Fac" << cby->id();
- found = found || (c == cby);
+ const auto& fo = fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> f" << fo->id() << " <- ";
+ for (auto cby : fo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained feature
+ bool found = fo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
}
- if (verbose)
- stream << ")";
- // check constrained_by pointer in constrained feature
- // is_consistent = is_consistent && found;
-
- inconsistency_explanation << "The constrained feature" << fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
+ if (_verbose && !c->getFeatureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained landmark
- if ( Lo ) // case LANDMARK:
+ for (const auto& Low : c->getLandmarkOtherList())
{
- if (verbose)
- stream << " ( --> Lmk" << Lo->id() << " <- ";
- bool found = false;
- for (auto cby : Lo->getConstrainedByList())
+ if (Low.expired())
{
- if (verbose)
- stream << " Fac" << cby->id();
- found = found || (c == cby);
+ const auto& Lo = Low.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> L" << Lo->id() << " <- ";
+ for (auto cby : Lo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained landmark
+ bool found = Lo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
}
- if (verbose)
- stream << ")";
- // check constrained_by pointer in constrained landmark
- // is_consistent = is_consistent && found;
-
- inconsistency_explanation << "The constrained landmark " << Lo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
- if (verbose)
- stream << std::endl;
+ if (_verbose && !c->getLandmarkOtherList().empty())
+ _stream << ")";
- if (verbose)
+ if (_verbose)
+ _stream << std::endl;
+
+ if (_verbose)
{
- stream << " -> Prb @ " << c->getProblem().get() << std::endl;
- stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
+ _stream << " -> Prb @ " << c->getProblem().get() << std::endl;
+ _stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
}
- // check problem and feature pointers
- // is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- inconsistency_explanation << "The factor " << c << " has problem ptr " << c->getProblem().get()
- << " but problem ptr is " << P_raw << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check problem and feature pointers
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
+ << " but problem ptr is " << P.get() << "\n";
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (c->getFeature() == f);
- inconsistency_explanation << "The factor " << c << " has feature ptr " << c->getFeature()
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
<< " but it should have " << f << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
+
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
- if (verbose)
+ if (_verbose)
{
- stream << " sb @ " << sb.get();
+ _stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
}
bool found_here;
- std::vector<StateBlockPtr> sb_vec;
// find in own Frame
- sb_vec = F->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " F" << F->id();
+ found_here = F->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " F" << F->id();
found = found || found_here;
// find in own Capture
- sb_vec = C->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " C" << C->id();
+ found_here = C->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " C" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
if (!found_here)
for (auto FC : F->getCaptureList())
{
- sb_vec = FC->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " F" << F->id() << ".C" << FC->id();
+ found_here = FC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " F" << F->id() << ".C" << FC->id();
found = found || found_here;
}
// find in own Sensor
if (S)
{
- sb_vec = S->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " S" << S->id();
+ found_here = S->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " S" << S->id();
found = found || found_here;
}
+
// find in constrained Frame
- if (Fo){
- sb_vec = Fo->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " Fo" << Fo->id();
- found = found || found_here;
+ for (const auto& Fow : c->getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ found_here = Fo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Fo" << Fo->id();
+ found = found || found_here;
+
+ // find in feature other's captures
+ for (auto FoC : Fo->getCaptureList())
+ {
+ found_here = FoC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
+ }
+ }
}
// find in constrained Capture
- if (Co)
+ for (const auto& Cow : c->getCaptureOtherList())
{
- sb_vec = Co->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " Co" << Co->id();
- found = found || found_here;
- }
-
- // Find in other Captures of the constrained Frame
- if (!found_here && Fo)
- for (auto FoC : Fo->getCaptureList())
+ if (!Cow.expired())
{
- sb_vec = FoC->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ const auto& Co = Cow.lock();
+ found_here = Co->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Co" << Co->id();
found = found || found_here;
}
+ }
// find in constrained Feature
- if (fo)
+ for (const auto& fow : c->getFeatureOtherList())
{
- // find in constrained feature's Frame
- auto foF = fo->getFrame();
- sb_vec = foF->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " foF" << foF->id();
- found = found || found_here;
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ // find in constrained feature's Frame
+ auto foF = fo->getFrame();
+ found_here = foF->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foF" << foF->id();
+ found = found || found_here;
- // find in constrained feature's Capture
- auto foC = fo->getCapture();
- sb_vec = foC->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " foC" << foC->id();
- found = found || found_here;
+ // find in constrained feature's Capture
+ auto foC = fo->getCapture();
+ found_here = foC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foC" << foC->id();
+ found = found || found_here;
- // find in constrained feature's Sensor
- auto foS = fo->getCapture()->getSensor();
- sb_vec = foS->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " foS" << foS->id();
- found = found || found_here;
+ // find in constrained feature's Sensor
+ auto foS = fo->getCapture()->getSensor();
+ found_here = foS->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foS" << foS->id();
+ found = found || found_here;
+ }
}
// find in constrained landmark
- if (Lo)
+ for (const auto& Low : c->getLandmarkOtherList())
{
- sb_vec = Lo->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) stream << " Lo" << Lo->id();
- found = found || found_here;
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ found_here = Lo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Lo" << Lo->id();
+ found = found || found_here;
+ }
}
- if (verbose)
+ if (_verbose)
{
if (found)
- stream << " found";
+ _stream << " found";
else
- stream << " NOT FOUND !";
- stream << std::endl;
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
}
+ // check that the state block has been found somewhere
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- inconsistency_explanation << "The stateblock " << sb << " of factor " << c << " is null\n";
- log.compose(CheckLog((sb.get() != nullptr), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((found), inconsistency_explanation);
- // check that all state block pointers were found
- // is_consistent = is_consistent && found;
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
}
}
}
@@ -1755,98 +1748,93 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose)
- stream << "Map @ " << M.get() << std::endl;
- // check pointer to Problem
- // is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+ if (_verbose)
+ _stream << "Map @ " << M.get() << std::endl;
- inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check pointer to Problem
+ inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P.get() << "\n";
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
- stream << " -> Prb @ " << L->getProblem().get() << std::endl;
- stream << " -> Map @ " << L->getMap().get() << std::endl;
+ _stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
+ _stream << " -> Prb @ " << L->getProblem().get() << std::endl;
+ _stream << " -> Map @ " << L->getMap().get() << std::endl;
for (const auto& pair : L->getStateBlockMap())
{
auto sb = pair.second;
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
- // check problem and map pointers
- // is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- inconsistency_explanation << "The landmarks problem ptr is "
+ // check problem and map pointers
+ inconsistency_explanation << "Landmarks' problem ptr is "
<< L->getProblem().get() << " but problem is "
- << P_raw << "\n";
-
- log.compose(CheckLog((L->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << P.get() << "\n";
- // is_consistent = is_consistent && (L->getMap() == M);
+ log.assertTrue((L->getProblem() == P), inconsistency_explanation);
- inconsistency_explanation << "The landmarks map ptr is "
+ inconsistency_explanation << "The Landmarks' map ptr is "
<< L->getMap() << " but map is "
<< M << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- for (auto cby : L->getConstrainedByList()) {
- if (verbose)
- stream << " <- Fac" << cby->id() << " -> Lmk"
- << cby->getLandmarkOther()->id() << std::endl;
- // check constrained_by pointer to this landmark
- // is_consistent =
- // is_consistent && (cby->getLandmarkOther() &&
- // L->id() == cby->getLandmarkOther()->id());
-
- inconsistency_explanation << "The landmark constrained-by loop started at "
- << cby
- << " is not closed\n";
- log.compose(CheckLog((cby->getLandmarkOther() &&
- L->id() == cby->getLandmarkOther()->id()), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
+
+ for (auto cby : L->getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << " <- Fac" << cby->id() << " ->";
+
+ for (const auto& Low : cby->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ _stream << " Lmk" << Lo->id();
+ }
+ }
+ _stream << std::endl;
+ }
+
+ // check constrained-by factors
+ inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
- if (verbose) {
- stream << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
}
}
- if (verbose) stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
- if (verbose) stream << std::endl;
- if (verbose and not log.is_consistent_) stream << log.explanation_ << std::endl;
+ if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
+ if (_verbose) _stream << std::endl;
+ if (_verbose and not log.is_consistent_) _stream << log.explanation_ << std::endl;
return log.is_consistent_;
}
-bool Problem::check(int verbose_level) const
+bool Problem::check(int _verbose_level) const
{
- return check((verbose_level > 0), std::cout);
+ return check((_verbose_level > 0), std::cout);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
{
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d32d5c6216c857ad703c34aa1a10f4b29d6c4dc0..db82caf1fc34b83b2509fe7d4f3ac4d6cc9e8c82 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -47,13 +47,9 @@ add_library(dummy ${SRC_DUMMY})
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_NAME})
-# CaptureBase class test
-#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
-#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
-
-# FactorBlockDifference class test
-wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+# FactorBase class test
+wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
+target_link_libraries(gtest_factor_base ${PROJECT_NAME})
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
@@ -169,6 +165,10 @@ target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME})
+# FactorBlockDifference class test
+wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
+target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af
--- /dev/null
+++ b/test/gtest_factor_base.cpp
@@ -0,0 +1,120 @@
+/*
+ * gtest_factor_base.cpp
+ *
+ * Created on: Apr 2, 2020
+ * Author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/factor/factor_base.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class FactorBaseTest : public testing::Test
+{
+ public:
+ FrameBasePtr F0,F1;
+ CaptureBasePtr C0,C1;
+ FeatureBasePtr f0,f1;
+ LandmarkBasePtr L0,L1;
+
+ virtual void SetUp()
+ {
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
+ C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
+ f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ f1 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ L0 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ L1 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ }
+// virtual void TearDown(){}
+};
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ _frame_other,
+ _capture_other,
+ _feature_other,
+ _landmark_other,
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ virtual ~FactorDummy() = default;
+
+ virtual std::string getTopology() const override {return "DUMMY";}
+ virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;}
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
+ virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
+ virtual unsigned int getSize() const override {return 0;}
+
+};
+
+TEST_F(FactorBaseTest, constructor_from_pointers)
+{
+ FactorDummy fac(nullptr,C0,f0,nullptr);
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+}
+
+TEST_F(FactorBaseTest, constructor_from_lists)
+{
+ FactorDummy fac({},{C0},{f0,f1},{});
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index d8b46e9b721fa4f9947138eee2655ed3ad25bff0..df90859ba57d46382d97e7fa29fa66b23b137580 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -162,6 +162,23 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
}
+TEST_F(HasStateBlocksTest, hasStateBlock)
+{
+ ASSERT_TRUE(F0->hasStateBlock(sbp0));
+ ASSERT_FALSE(F0->hasStateBlock(sbv1));
+}
+
+TEST_F(HasStateBlocksTest, stateBlockKey)
+{
+ std::string key;
+ ASSERT_TRUE(F0->stateBlockKey(sbp0, key));
+ ASSERT_TRUE(key == "P");
+
+ ASSERT_FALSE(F0->stateBlockKey(sbp1, key));
+ ASSERT_TRUE(key == "");
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index ae7e14ae893449e8c42df4736eae792dfbd4b20d..6d262ba06b81438499b29ab3d45fd4bb101c89db 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -19,8 +19,12 @@
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
+//#include "core/feature/feature_diff_drive.h"
+#include "core/factor/factor_diff_drive.h"
+
#include "core/state_block/state_quaternion.h"
+
#include <iostream>
using namespace wolf;
@@ -413,6 +417,70 @@ TEST(Problem, perturb)
}
}
+TEST(Problem, check)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic("I", true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(KEY, pose, t);
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
+
+ for (int k = 0; k<2; k++)
+ {
+ Vector3d d(1,2,3), c(1,1,1);
+ Matrix3d D = Matrix3d::Identity(), J=Matrix3d::Zero();
+ auto fea = FeatureBase::emplace<FeatureMotion>(cap, "FeatureDiffDrive", d, D, c, J);
+ auto fac = FactorBase::emplace<FactorDiffDrive>(fea, fea, cap, nullptr, false);
+ }
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ // remove stuff from problem, and re-check
+
+ auto F = problem->getLastKeyFrame();
+
+ auto cby = F->getConstrainedByList().front();
+
+ cby->remove(true);
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ F->remove();
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);