Skip to content
Snippets Groups Projects
Commit 89f00c17 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch '294-frame-capture-feature-landmark-other-as-list-vectors-in-factorbase' into 'devel'

Resolve "Frame/Capture/Feature/Landmark Other as list/vectors in FactorBase"

Closes #294

See merge request !346
parents b0ade8fc 48482444
No related branches found
No related tags found
1 merge request!346Resolve "Frame/Capture/Feature/Landmark Other as list/vectors in FactorBase"
Pipeline #5086 passed
Showing
with 1103 additions and 581 deletions
......@@ -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_;
......
......@@ -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; \
......
......@@ -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();
......
......@@ -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;
}
......
......@@ -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;
......
......@@ -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);
......
......@@ -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);
......
......@@ -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_ */
......@@ -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
......@@ -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())
......
......@@ -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)
......
......@@ -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_;
......
......@@ -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");
......
......@@ -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 >();
......
This diff is collapsed.
......@@ -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})
......
/*
* 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();
}
......@@ -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);
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment