diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6f9c6e3e2ca856c90b41f8e45e7981f221e0c440..98de6dbd9bfb7521238c7c784b077456b35b5629 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -181,14 +181,14 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_UTILS
- include/core/utils/check_log.hpp
+ include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
- include/core/utils/loader.hpp
+ include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/make_unique.h
- include/core/utils/params_server.hpp
+ include/core/utils/params_server.h
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
include/core/utils/converter_utils.h
@@ -291,7 +291,7 @@ SET(HDRS_TREE_MANAGER
include/core/tree_manager/tree_manager_sliding_window.h
)
SET(HDRS_YAML
- include/core/yaml/parser_yaml.hpp
+ include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
)
#SOURCES
@@ -325,6 +325,9 @@ SET(SRCS_MATH
)
SET(SRCS_UTILS
src/utils/converter_utils.cpp
+ src/utils/params_server.cpp
+ src/utils/loader.cpp
+ src/utils/check_log.cpp
)
SET(SRCS_CAPTURE
@@ -376,6 +379,7 @@ SET(SRCS_TREE_MANAGER
src/tree_manager/tree_manager_sliding_window.cpp
)
SET(SRCS_YAML
+ src/yaml/parser_yaml.cpp
src/yaml/processor_odom_3d_yaml.cpp
src/yaml/sensor_odom_2d_yaml.cpp
src/yaml/sensor_odom_3d_yaml.cpp
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index a97d8d2f9c5c7fb8c7f1f37c12330038beb3ba98..a387adfe9656523c37d025bb3a74f0f198281b3d 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -9,7 +9,7 @@
// wolf core includes
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
#include "core/ceres_wrapper/ceres_manager.h"
// hello wolf local includes
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e62bfc1a5cd4ff7115cac06f9738204c71017f2b..491efff4f533edc993051482de7d0e84562a790a 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index aaa76c9220c4351f1cca5b6a38bf290ef8e69c58..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;
@@ -101,7 +103,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
protected:
- SizeEigen computeCalibSize() const;
+ virtual SizeEigen computeCalibSize() const;
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
@@ -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/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index f124325950a06f74642cff38b6d0c053958cab8e..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -8,7 +8,7 @@
//wolf includes
#include "core/solver/solver_manager.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/ceres_wrapper/cost_function_wrapper.h"
#include "core/ceres_wrapper/local_parametrization_wrapper.h"
#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
@@ -84,6 +84,8 @@ class CeresManager : public SolverManager
void check();
+ const Eigen::SparseMatrixd computeHessian() const;
+
protected:
std::string solveImpl(const ReportVerbosity report_level) override;
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 5295bc0ac17f941503a8f28e462962290a9ee82b..80f6516415e28ddce47ab272415604014ffb0a66 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -1,7 +1,7 @@
#ifndef PARAMS_BASE_H_
#define PARAMS_BASE_H_
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
struct ParamsBase
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 741ad7f155884175aafb1b8183f8c672842abc1e..b11165468e47ef55e7c65ce046571d7e68ebd945 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_block_absolute.h b/include/core/factor/factor_block_absolute.h
index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
+ FactorAnalytic("FactorBlockAbsolute",
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8f62b63838d79c6a654c80120bff4e4a144f4098..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
*
*/
FactorBlockDifference(
- StateBlockPtr _sb1_ptr,
- StateBlockPtr _sb2_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
unsigned int _start_idx1 = 0,
int _size1 = -1,
unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
+ FactorAnalytic("FactorBlockDifference",
+ _frame_other,
+ _cap_other,
+ _feat_other,
+ _lmk_other,
_processor_ptr,
_apply_loss_function,
_status,
@@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
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/problem/problem.h b/include/core/problem/problem.h
index e0463b12f2d09b370375b9aa2bdb57894a134ca2..edda0b3b59fa839cdb6c5156abf61e1f2c338174 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -18,7 +18,7 @@ struct ProcessorParamsBase;
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/processor/is_motion.h"
@@ -47,8 +47,7 @@ class Problem : public std::enable_shared_from_this<Problem>
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- IsMotionPtr processor_motion_ptr_;
-// IsMotionPtr is_motion_ptr_;
+ std::list<IsMotionPtr> processor_is_motion_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -152,15 +151,15 @@ class Problem : public std::enable_shared_from_this<Problem>
protected:
/** \brief Set the processor motion
*
- * Set the processor motion.
+ * Add a new processor of type is motion to the processor is motion list.
*/
- void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
- IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
- void clearProcessorMotion();
+ void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
+ void clearProcessorIsMotion(IsMotionPtr proc);
public:
- IsMotionPtr& getProcessorMotion();
-
+ IsMotionPtr getProcessorIsMotion();
+ std::list<IsMotionPtr> getProcessorIsMotionList();
+
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
@@ -276,10 +275,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
bool priorIsSet() const;
+ void setPriorIsSet(bool _prior_is_set);
// Perturb state
void perturb(double amplitude = 0.01);
-
// Map branch -----------------------------------------
MapBasePtr getMap() const;
void loadMap(const std::string& _filename_dot_yaml);
@@ -377,9 +376,22 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline IsMotionPtr& Problem::getProcessorMotion()
+inline void Problem::setPriorIsSet(bool _prior_is_set)
+{
+ prior_is_set_ = _prior_is_set;
+}
+
+inline IsMotionPtr Problem::getProcessorIsMotion()
+{
+ if (!processor_is_motion_list_.empty())
+ return processor_is_motion_list_.front();
+ return nullptr;
+}
+
+
+inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
{
- return processor_motion_ptr_;
+ return processor_is_motion_list_;
}
inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index f3cf0d781f39c9fda6ffb9e64817ceed0acfa162..8c99fe72e37698a2210e35a3af0c14bf9168f65e 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -43,6 +43,12 @@ class IsMotion
TimeStamp getCurrentTimeStamp() const;
Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ std::string getStateStructure(){return state_structure_;};
+ void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+
+ protected:
+ std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+
};
}
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index f3feeb56a3bcc6787d8efeb1355175b989dd3f3e..dc62abcac8375fc238f7c93617a33ccb8662d246 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -117,13 +117,18 @@ public:
*
* elements are ordered from most recent to oldest
*/
- std::map<TimeStamp,T> getContainer();
+ const std::map<TimeStamp,T>& getContainer();
/**\brief Remove all packs in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpToLower(const TimeStamp& _time_stamp);
+
/**\brief Clear the buffer
*
*/
@@ -503,7 +508,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
}
template <typename T>
-std::map<TimeStamp,T> Buffer<T>::getContainer()
+const std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
return container_;
}
@@ -533,6 +538,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
container_.erase(container_.begin(), post); // erasing by range
}
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.lower_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
const TimeStamp& _time_stamp2, const double& _time_tolerance2)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 2ac6c8d5949ad07c1529fd06739c0ce80211f243..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -13,7 +13,7 @@
#include "core/processor/processor_base.h"
#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
// std
#include <iomanip>
@@ -148,6 +148,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
@@ -505,11 +506,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
+ _x.resize( curr_x.size() );
// do get timestamp and state corrected by possible self-calibrations
double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+ statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 6d46ab61a32c1b9573f386c97f1ae671086478c6..b03416837ec1b0a6a86a67ff0cf40fdf820b1370 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -12,7 +12,7 @@
#include "core/capture/capture_odom_2d.h"
#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
index 3930176443a22cebb8c9591253af7f58b0a40a2c..daf01499ac759d799517cdd73ab830fb6ccaf4d5 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_factory.h
@@ -16,7 +16,7 @@ struct ParamsSensorBase;
// wolf
#include "core/common/factory.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index d6a431d3102b0a82a218ee71247552066c1ef8eb..45499f5c56fb342ed8a82b8521e6c2e156d06031 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -3,7 +3,7 @@
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index 67efe1e9af27f8456401c400766dcbf32f44057b..3d08375b4fc30e43f07f201a5653d5e83a258381 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -10,7 +10,7 @@
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ef55d163be4764393c4e1b08ac0003ae67b1d223..07c6d757bc09125c4a4e8a55aefd6d37b480c7d4 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -15,6 +15,10 @@
namespace wolf
{
+/// State of nodes containing several state blocks
+typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+
+
class HasStateBlocks
{
public:
@@ -44,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>
@@ -62,11 +69,13 @@ class HasStateBlocks
void removeStateBlocks(ProblemPtr _problem);
// States
- virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
- Eigen::VectorXd getState() const;
- void getState(Eigen::VectorXd& _state) const;
- unsigned int getSize() const;
- unsigned int getLocalSize() const;
+ inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
+ void getState(Eigen::VectorXd& _state, std::string structure="") const;
+ Eigen::VectorXd getState(std::string structure="") const;
+ unsigned int getSize(std::string _sub_structure="") const;
+ unsigned int getLocalSize(std::string _sub_structure="") const;
+
+ State getStateComposite();
// Perturb state
void perturb(double amplitude = 0.01);
@@ -198,62 +207,145 @@ inline bool HasStateBlocks::isFixed() const
return fixed;
}
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
{
- int size = getSize();
-
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
+ int size = getSize(_sub_structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char key : getStructure())
+ for (const char key : _sub_structure)
{
const auto& sb = getStateBlock(key);
-
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
+// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
{
- _state.resize(getSize());
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
+ _state.resize(getSize(_sub_structure));
unsigned int index = 0;
- for (const char key : getStructure())
+ for (const char key : _sub_structure)
{
const auto& sb = getStateBlock(key);
-
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
_state.segment(index,sb->getSize()) = sb->getState();
index += sb->getSize();
}
}
-inline Eigen::VectorXd HasStateBlocks::getState() const
+inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
{
Eigen::VectorXd state;
- getState(state);
+ getState(state, _sub_structure);
return state;
}
-inline unsigned int HasStateBlocks::getSize() const
+inline State HasStateBlocks::getStateComposite()
{
+ State state;
+ for (auto& pair_key_kf : state_block_map_)
+ {
+ state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+ }
+ return state;
+}
+
+inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
+{
+ if (_sub_structure == ""){
+ _sub_structure = structure_;
+ }
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getSize();
+ }
+
return size;
}
-inline unsigned int HasStateBlocks::getLocalSize() const
+//<<<<<<< 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_;
+ }
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getLocalSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getLocalSize();
+ }
+
return size;
}
} // namespace wolf
+
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d..ab3bb7147f3c81f57a3436216412274394acc04a 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
**/
Eigen::VectorXd getState() const;
+ /** \brief Returns the state vector data array
+ **/
+ double* getStateData();
+
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -283,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated()
local_param_updated_.store(false);
}
+inline double* StateBlock::getStateData()
+{
+ return state_.data();
+}
+
}// namespace wolf
#endif
diff --git a/include/core/utils/check_log.h b/include/core/utils/check_log.h
new file mode 100644
index 0000000000000000000000000000000000000000..035bd0aa0e2e9fa4989c95509800d1fee517bd46
--- /dev/null
+++ b/include/core/utils/check_log.h
@@ -0,0 +1,22 @@
+#ifndef CHECK_LOG_H
+#define CHECK_LOG_H
+#include <iostream>
+#include <string>
+#include <sstream>
+
+namespace wolf
+{
+class CheckLog
+{
+ public:
+ bool is_consistent_;
+ std::string explanation_;
+
+ CheckLog();
+ CheckLog(bool _consistent, std::string _explanation);
+ ~CheckLog(){};
+ void compose(CheckLog l);
+ void assertTrue(bool _condition, std::stringstream& _stream);
+};
+} // namespace wolf
+#endif
diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
deleted file mode 100644
index db3ab2dc9dd6e165c3541544bf7770022568f2fe..0000000000000000000000000000000000000000
--- a/include/core/utils/check_log.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef CHECK_LOG_HPP
-#define CHECK_LOG_HPP
-#include <iostream>
-#include <string>
-
-class CheckLog {
-
-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_;
- }
-};
-#endif
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f1a4cc68c2d0dcb20a6ef18f53ec259507fc176
--- /dev/null
+++ b/include/core/utils/loader.h
@@ -0,0 +1,32 @@
+#ifndef LOADER_H
+#define LOADER_H
+
+#include <string>
+
+class Loader{
+protected:
+ std::string path_;
+public:
+ Loader(std::string _file);
+ virtual void load() = 0;
+ virtual void close() = 0;
+};
+
+class LoaderRaw: public Loader{
+ void* resource_;
+public:
+ LoaderRaw(std::string _file);
+ ~LoaderRaw();
+ void load();
+ void close();
+};
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// this->resource_ = new ClassLoader(this->path_);
+// }
+// void close(){
+// delete this->resource_;
+// }
+// };
+#endif
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
deleted file mode 100644
index 3d867d799b36bfd53fb42e93577554caf1b050c1..0000000000000000000000000000000000000000
--- a/include/core/utils/loader.hpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
-#include <string>
-#include <stdexcept>
-class Loader{
-protected:
- std::string path_;
-public:
- Loader(std::string _file){
- this->path_ = _file;
- }
- virtual void load() = 0;
- virtual void close() = 0;
-};
-class LoaderRaw: public Loader{
- void* resource_;
-public:
- LoaderRaw(std::string _file):
- Loader(_file)
- {
- //
- }
- ~LoaderRaw(){
- this->close();
- }
- void load(){
- this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY);
- if(not this->resource_)
- throw std::runtime_error("Couldn't load resource with path " + this->path_ + "\n" + "Error info: " + dlerror());
- }
- void close(){
- if(this->resource_) dlclose(this->resource_);
- }
-};
-// class LoaderPlugin: public Loader{
-// ClassLoader* resource_;
-// void load(){
-// this->resource_ = new ClassLoader(this->path_);
-// }
-// void close(){
-// delete this->resource_;
-// }
-// };
-#endif
\ No newline at end of file
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026
--- /dev/null
+++ b/include/core/utils/params_server.h
@@ -0,0 +1,55 @@
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
+
+#include "core/utils/converter.h"
+
+#include <map>
+#include <exception>
+
+namespace wolf{
+
+class MissingValueException : public std::runtime_error
+{
+public:
+ MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
+};
+
+class ParamsServer{
+ std::map<std::string, std::string> params_;
+public:
+ ParamsServer();
+ ParamsServer(std::map<std::string, std::string> _params);
+ ~ParamsServer(){
+ //
+ }
+
+ void print();
+
+
+ void addParam(std::string _key, std::string _value);
+
+ void addParams(std::map<std::string, std::string> _params);
+
+ // template<typename T>
+ // T getParam(std::string key, std::string def_value) const {
+ // if(params_.find(key) != params_.end()){
+ // return converter<T>::convert(params_.find(key)->second);
+ // }else{
+ // return converter<T>::convert(def_value);
+ // }
+ // }
+
+ template<typename T>
+ T getParam(std::string _key) const {
+ if(params_.find(_key) != params_.end()){
+ return converter<T>::convert(params_.find(_key)->second);
+ }else{
+ throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
+ }
+ }
+
+};
+
+}
+
+#endif
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
deleted file mode 100644
index a750eab79f4ee90b39e4e21d1926129897c29d92..0000000000000000000000000000000000000000
--- a/include/core/utils/params_server.hpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
-
-#include "core/utils/converter.h"
-//#include "core/yaml/parser_yaml.hpp"
-
-#include <vector>
-#include <regex>
-#include <map>
-#include <exception>
-
-namespace wolf{
-
-class MissingValueException : public std::runtime_error
-{
-public:
- MissingValueException(std::string msg) : std::runtime_error(msg) {}
-};
-
-class ParamsServer{
- std::map<std::string, std::string> _params;
-public:
- ParamsServer(){
- _params = std::map<std::string, std::string>();
- }
- ParamsServer(std::map<std::string, std::string> params){
- _params = params;
- }
- ~ParamsServer(){
- //
- }
-
- void print(){
- for(auto it : _params)
- std::cout << it.first << "~~" << it.second << std::endl;
- }
-
-
- void addParam(std::string key, std::string value){
- _params.insert(std::pair<std::string, std::string>(key, value));
- }
-
- void addParams(std::map<std::string, std::string> params)
- {
- _params.insert(params.begin(), params.end());
- }
-
- // template<typename T>
- // T getParam(std::string key, std::string def_value) const {
- // if(_params.find(key) != _params.end()){
- // return converter<T>::convert(_params.find(key)->second);
- // }else{
- // return converter<T>::convert(def_value);
- // }
- // }
-
- template<typename T>
- T getParam(std::string key) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
- }
- }
-
-};
-
-}
-
-#endif
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b6f659ebd609dffef4f3c472800cf32714f22e1
--- /dev/null
+++ b/include/core/yaml/parser_yaml.h
@@ -0,0 +1,80 @@
+#ifndef PARSER_YAML_H
+#define PARSER_YAML_H
+
+#include "core/utils/converter.h"
+#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class ParserYAML {
+ struct ParamsInitSensor{
+ std::string type_;
+ std::string name_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct ParamsInitProcessor{
+ std::string type_;
+ std::string name_;
+ std::string name_assoc_sensor_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct SubscriberManager{
+ std::string package_;
+ std::string subscriber_;
+ std::string topic_;
+ std::string sensor_name_;
+ YAML::Node n_;
+ };
+ struct PublisherManager{
+ std::string subscriber_;
+ std::string topic_;
+ std::string period_;
+ YAML::Node n_;
+ };
+ std::map<std::string, std::string> params_;
+ std::string active_name_;
+ std::vector<ParamsInitSensor> paramsSens_;
+ std::vector<ParamsInitProcessor> paramsProc_;
+ std::stack<std::string> parsing_file_;
+ std::string file_;
+ std::string path_root_;
+ std::vector<SubscriberManager> subscriber_managers_;
+ std::vector<PublisherManager> publisher_managers_;
+ YAML::Node problem;
+ std::string generatePath(std::string);
+ YAML::Node loadYAML(std::string);
+ void insert_register(std::string, std::string);
+public:
+ ParserYAML(std::string _file, std::string _path_root = "",
+ bool _freely_parse = false);
+ ~ParserYAML()
+ {
+ //
+ }
+ void parse_freely();
+ std::map<std::string, std::string> getParams();
+
+ private:
+ void walkTree(std::string _file);
+ void walkTree(std::string _file, std::vector<std::string>& _tags);
+ void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr);
+/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
+ * @param YAML node to be parsed
+ * @param tags represents the path from the root of the YAML tree to the current node
+ * @param hdr is the name of the current YAML node
+ */
+ void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
+ void updateActiveName(std::string _tag);
+/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
+ are defined at the top level of the YAML file.
+ * @param file is the path to the YAML file */
+ void parseFirstLevel(std::string _file);
+ std::string tagsToString(std::vector<std::string>& _tags);
+ void parse();
+};
+}
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
deleted file mode 100644
index 22f584d76bab2aa2c3a6bd6ca30edbeb4873a4ea..0000000000000000000000000000000000000000
--- a/include/core/yaml/parser_yaml.hpp
+++ /dev/null
@@ -1,532 +0,0 @@
-#ifndef PARSER_YAML_HPP
-#define PARSER_YAML_HPP
-
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-
-#include "yaml-cpp/yaml.h"
-
-#include <string>
-#include <vector>
-#include <list>
-#include <stack>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-#include <numeric>
-
-namespace {
- //====== START OF FORWARD DECLARATION ========
- std::string parseAtomicNode(YAML::Node);
- std::string fetchMapEntry(YAML::Node);
- std::string mapToString(std::map<std::string,std::string>);
- //====== END OF FORWARD DECLARATION ========
-
-/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
- * @param n the node representing a map
- * @return std::map<std::string, std::string> populated with the key,value pairs in n
- */
-std::map<std::string, std::string> fetchAsMap(YAML::Node n){
- assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
- auto m = std::map<std::string, std::string>();
- for(const auto& kv : n){
- std::string key = kv.first.as<std::string>();
- switch (kv.second.Type()) {
- case YAML::NodeType::Scalar : {
- std::string value = kv.second.Scalar();
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- case YAML::NodeType::Sequence : {
- std::string aux = parseAtomicNode(kv.second);
- m.insert(std::pair<std::string,std::string>(key, aux));
- break;
- }
- case YAML::NodeType::Map : {
- std::string value = fetchMapEntry(kv.second);
- std::regex r("^\\$.*");
- if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at fetchAsMap");
- break;
- }
- }
- return m;
-}
- std::string fetchMapEntry(YAML::Node n){
- switch (n.Type()) {
- case YAML::NodeType::Scalar: {
- return n.Scalar();
- break;
- }
- case YAML::NodeType::Sequence: {
- return parseAtomicNode(n);
- break;
- }
- case YAML::NodeType::Map: {
- return mapToString(fetchAsMap(n));
- break;
- }
- default: {
- assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
- return "";
- break;
- }
- }
- }
- /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
- * @param _map just a std::map<std::string,std::string>
- * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
- */
- std::string mapToString(std::map<std::string,std::string> _map){
- std::string result = "";
- auto v = std::vector<std::string>();
- std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
- auto concat = [](std::string ac, std::string str)-> std::string {
- return ac + str + ",";
- };
- std::string aux = "";
- std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- return "[" + accumulated + "]";
- }
- /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
- * @param n a vector of YAML::Node that represents a YAML::Sequence
- * @return <b>{std::string}</b> representing the YAML sequence
- */
- std::string parseAtomicNode(YAML::Node n){
- std::string aux = "";
- bool first = true;
- std::string separator = "";
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return n.Scalar();
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n){
- aux += separator + parseAtomicNode(it);
- if(first){
- separator = ",";
- first = false;
- }
- }
- return "[" + aux + "]";
- break;
- case YAML::NodeType::Map:
- return mapToString(fetchAsMap(n));
- break;
- default:
- return "";
- break;
- }
- }
-
- /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
- * Scalar, Sequence or Map.
- * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
- * @param n node to be test for atomicity
- */
- bool isAtomic(std::string key, YAML::Node n){
- assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
- std::regex r("^\\$.*");
- bool is_atomic = true;
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return true;
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n) {
- switch(it.Type()){
- case YAML::NodeType::Map:
- for(const auto& kv : it){
- is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
- }
- break;
- default:
- is_atomic = is_atomic and isAtomic("", it);
- break;
- }
- }
- return is_atomic;
- break;
- case YAML::NodeType::Map:
- is_atomic = std::regex_match(key, r);
- return is_atomic;
- break;
- default:
- throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type()));
- return false;
- break;
- }
- return false;
- }
-}
-class ParserYAML {
- struct ParamsInitSensor{
- std::string _type;
- std::string _name;
- std::string _plugin;
- YAML::Node n;
- };
- struct ParamsInitProcessor{
- std::string _type;
- std::string _name;
- std::string _name_assoc_sensor;
- std::string _plugin;
- YAML::Node n;
- };
- struct SubscriberManager{
- std::string _package;
- std::string _subscriber;
- std::string _topic;
- std::string _sensor_name;
- YAML::Node n;
- };
- struct PublisherManager{
- std::string _subscriber;
- std::string _topic;
- std::string _period;
- YAML::Node n;
- };
- std::map<std::string, std::string> _params;
- std::string _active_name;
- std::vector<ParamsInitSensor> _paramsSens;
- std::vector<ParamsInitProcessor> _paramsProc;
- std::stack<std::string> _parsing_file;
- std::string _file;
- std::string _path_root;
- std::vector<SubscriberManager> _subscriber_managers;
- std::vector<PublisherManager> _publisher_managers;
- YAML::Node problem;
- std::string generatePath(std::string);
- YAML::Node loadYAML(std::string);
- void insert_register(std::string, std::string);
-public:
- ParserYAML(std::string file, std::string path_root = "",
- bool freely_parse = false) {
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _subscriber_managers = std::vector<SubscriberManager>();
- _publisher_managers = std::vector<PublisherManager>();
- _parsing_file = std::stack<std::string>();
- _file = file;
- if (path_root != "") {
- std::regex r(".*/ *$");
- if (not std::regex_match(path_root, r))
- _path_root = path_root + "/";
- else
- _path_root = path_root;
- }
- if(not freely_parse) this->parse();
- else this->parse_freely();
- }
- ~ParserYAML()
- {
- //
- }
- void parse_freely();
- std::map<std::string, std::string> getParams();
-
- private:
- void walkTree(std::string file);
- void walkTree(std::string file, std::vector<std::string>& tags);
- void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr);
- void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr);
- void updateActiveName(std::string tag);
- void parseFirstLevel(std::string file);
- std::string tagsToString(std::vector<std::string>& tags);
- void parse();
-};
-std::string ParserYAML::generatePath(std::string file){
- std::regex r("^/.*");
- if(std::regex_match(file, r)){
- return file;
- }else{
- return _path_root + file;
- }
-}
-YAML::Node ParserYAML::loadYAML(std::string file){
- try{
- // std::cout << "Parsing " << generatePath(file) << std::endl;
- WOLF_INFO("Parsing file: ", generatePath(file));
- return YAML::LoadFile(generatePath(file));
- }catch (YAML::BadFile& e){
- throw std::runtime_error("Couldn't load file " + generatePath(file) + ". Tried to open it from " + this->_parsing_file.top());
- }
-}
-std::string ParserYAML::tagsToString(std::vector<std::string> &tags){
- std::string hdr = "";
- for(auto it : tags){
- hdr = hdr + "/" + it;
- }
- return hdr;
-}
-void ParserYAML::walkTree(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- std::vector<std::string> hdrs = std::vector<std::string>();
- walkTreeR(n, hdrs, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, hdr);
- this->_parsing_file.pop();
-}
-/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
-* @param YAML node to be parsed
-* @param tags represents the path from the root of the YAML tree to the current node
-* @param hdr is the name of the current YAML node
-*/
-void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){
- switch (n.Type()) {
- case YAML::NodeType::Scalar : {
- std::regex r("^@.*");
- if(std::regex_match(n.Scalar(), r)){
- std::string str = n.Scalar();
- walkTree(str.substr(1,str.size() - 1), tags, hdr);
- }else{
- insert_register(hdr, n.Scalar());
- }
- break;
- }
- case YAML::NodeType::Sequence : {
- if(isAtomic("", n)){
- insert_register(hdr, parseAtomicNode(n));
- }else{
- for(const auto& kv : n){
- walkTreeR(kv, tags, hdr);
- }
- }
- break;
- }
- case YAML::NodeType::Map : {
- for(const auto& kv : n){
- if(isAtomic(kv.first.as<std::string>(), n)){
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
- key = key.substr(1,key.size() - 1);
- insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
- }else{
- /*
- If key=="follow" then the parser will assume that the value is a path and will parse
- the (expected) yaml file at the specified path. Note that this does not increase the header depth.
- The following example shows how the header remains unafected:
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - follow: "@some_path" |
- - var: 1.2 |
- Resulting map:
- cov_det -> 1
- my_value-> 23
- var: 1.2
- Instead of:
- cov_det -> 1
- follow/my_value-> 23
- var: 1.2
- Which would result from the following yaml files
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - $follow: "@some_path" |
- - var: 1.2 |
- */
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
- std::regex rr("follow");
- if(not std::regex_match(kv.first.as<std::string>(), rr)) {
- tags.push_back(kv.first.as<std::string>());
- if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>());
- walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>());
- tags.pop_back();
- if(tags.size() == 1) this->updateActiveName("");
- }else{
- walkTree(kv.second.as<std::string>(), tags, hdr);
- }
- }
- }
- break;
- }
- case YAML::NodeType::Null :
- break;
- default:
- assert(1 == 0 && "Unsupported node Type at walkTreeR.");
- break;
- }
-}
-void ParserYAML::updateActiveName(std::string tag){
- this->_active_name = tag;
-}
-/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
- are defined at the top level of the YAML file.
- * @param file is the path to the YAML file */
-void ParserYAML::parseFirstLevel(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(file));
-
- YAML::Node n_config = n["config"];
- // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
- if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'");
- if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry");
- this->problem = n_config["problem"];
- std::vector<std::map<std::string, std::string>> map_container;
- try{
- for(const auto& kv : n_config["sensors"]){
- ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsSens.push_back(pSensor);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()}
- }));
- }
- insert_register("sensors", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
- }
-
- try{
- for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsProc.push_back(pProc);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("processors",
- wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries.");
- }
- try {
- for (const auto &kv : n_config["ROS subscriber"]) {
- SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv};
- _subscriber_managers.push_back(pSubscriberManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"package", kv["package"].Scalar()},
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing subscriber @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
- }
-
- try {
- for (const auto &kv : n_config["ROS publisher"]) {
- WOLF_DEBUG("WHAT");
- PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv};
- _publisher_managers.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"period", kv["period"].Scalar()}}));
- }
- insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing publisher @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
- }
-}
-std::map<std::string,std::string> ParserYAML::getParams(){
- std::map<std::string,std::string> rtn = _params;
- return rtn;
-}
-void ParserYAML::parse(){
- this->_parsing_file.push(generatePath(this->_file));
- this->parseFirstLevel(this->_file);
-
- if(this->problem.Type() != YAML::NodeType::Undefined){
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(this->problem, tags , "problem");
- }
- for(auto it : _paramsSens){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("sensor");
- this->walkTreeR(it.n , tags , "sensor/" + it._name);
- }
- for(auto it : _paramsProc){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("processor");
- this->walkTreeR(it.n , tags , "processor/" + it._name);
- }
- std::list<std::string> plugins, packages;
- for (const auto& it : this->_paramsSens)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_paramsProc)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_subscriber_managers)
- packages.push_back(it._package);
- plugins.sort();
- plugins.unique();
- packages.sort();
- packages.unique();
- insert_register("plugins", wolf::converter<std::string>::convert(plugins));
- insert_register("packages", wolf::converter<std::string>::convert(packages));
-
- YAML::Node n;
- n = loadYAML(generatePath(this->_file));
-
- if (n.Type() == YAML::NodeType::Map)
- {
- for (auto it : n)
- {
- auto node = it.second;
- // WOLF_INFO("WUT ", it.first);
- std::vector<std::string> tags = std::vector<std::string>();
- if(it.first.as<std::string>() != "config")
- this->walkTreeR(node, tags, it.first.as<std::string>());
- else
- {
- for (auto itt : node)
- {
- std::string node_key = itt.first.as<std::string>();
- if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
- node_key != "ROS subscriber" and node_key != "ROS publisher")
- {
- this->walkTreeR(itt.second, tags, node_key);
- }
- }
- }
- }
- }else
- {
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(n, tags, "");
- }
- this->_parsing_file.pop();
- }
-void ParserYAML::parse_freely(){
- this->_parsing_file.push(generatePath(this->_file));
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(loadYAML(this->_file), tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::insert_register(std::string key, std::string value){
- if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
- auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
- if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
-}
-#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/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index ecf57e7d7e29b4b8ccfecf09f8c39d8eacdafdbb..0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -443,6 +443,77 @@ void CeresManager::check()
}
}
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+{
+ for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
+ for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
+ original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+
+ original.makeCompressed();
+}
+
+const Eigen::SparseMatrixd CeresManager::computeHessian() const
+{
+ Eigen::SparseMatrixd H;
+ Eigen::SparseMatrixd A;
+ // fac_2_residual_idx_
+ // fac_2_costfunction_
+ // state_blocks_local_param_
+ int ix_row = 0; // index of the factor/measurement in the
+ for (auto fac_res_pair: fac_2_residual_idx_){
+ FactorBasePtr fac_ptr = fac_res_pair.first;
+ ix_row += fac_ptr->getSize();
+
+ // retrieve all stateblocks data related to this factor
+ std::vector<const double*> fac_states_ptr;
+ for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+ fac_states_ptr.push_back(sb->getStateData());
+ }
+
+ // retrieve residual value, not used afterwards in this case since we just care about jacobians
+ Eigen::VectorXd residual(fac_ptr->getSize());
+ // retrieve jacobian in group size, not local size
+ std::vector<Eigen::MatrixXd> jacobians;
+ fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+
+ // Retrieve the block row sparse matrix of this factor
+ Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols());
+ for (auto i = 0; i < jacobians.size(); i++)
+ {
+ StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
+ if (!sb->isFixed())
+ {
+ assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
+ assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+
+ // insert since A_block_row has just been created so it's empty for sure
+ if (sb->hasLocalParametrization()){
+ // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+ Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+ Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+ Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+ sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+ insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
+ }
+ else {
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
+ }
+ }
+ }
+
+ // fill the weighted jacobian matrix block row
+ A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
+
+ }
+
+ // compute the hessian matrix from the weighted jacobian
+ H = A.transpose() * A;
+
+ return H;
+}
+
+
} // namespace wolf
#include "core/solver/solver_factory.h"
namespace wolf {
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 85f94bd2a97af8a68e4b1551d3352daa8e8906d8..c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -64,14 +64,14 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2d!");
+ assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
this->setType("PO 2d");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3d!");
+ assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
addStateBlock("P", p_ptr);
@@ -79,7 +79,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
this->setType("PO 3d");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3d!");
+ assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
@@ -87,6 +87,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
this->setType("POV 3d");
+ }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
+ // auto _x = Eigen::VectorXd::Zero(10);
+ assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
+ StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
+ StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) );
+ StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.segment <3> (7 ) ) );
+ StateBlockPtr c_ptr ( std::make_shared<StateBlock> (_x.segment <3> (10) ) );
+ StateBlockPtr cd_ptr ( std::make_shared<StateBlock> (_x.segment <3> (13) ) );
+ StateBlockPtr Lc_ptr ( std::make_shared<StateBlock> (_x.segment <3> (16) ) );
+ addStateBlock("P", p_ptr);
+ addStateBlock("O", o_ptr);
+ addStateBlock("V", v_ptr);
+ addStateBlock("C", c_ptr);
+ addStateBlock("D", cd_ptr);
+ addStateBlock("L", Lc_ptr);
+ this->setType("POVCDL 3d");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
@@ -298,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 550118c396c94561ce398d4cc1b9f3b2f8b31ed4..747737dc1ff0ea4b742a97017065d229889ea7d6 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -12,10 +12,15 @@
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
#include "core/tree_manager/factory_tree_manager.h"
+#include "core/tree_manager/tree_manager_base.h"
#include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
-#include "core/utils/loader.hpp"
-#include "core/utils/check_log.hpp"
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
+#include "core/utils/check_log.h"
+
+// IRI libs includes
+
+// C++ includes
#include <algorithm>
#include <map>
#include <sstream>
@@ -24,7 +29,6 @@
#include <vector>
#include <unordered_set>
-#include "../../include/core/tree_manager/tree_manager_base.h"
namespace wolf
{
@@ -34,25 +38,27 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
- processor_motion_ptr_(),
+ processor_is_motion_list_(std::list<IsMotionPtr>()),
prior_is_set_(false),
frame_structure_(_frame_structure)
{
+ dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
{
state_size_ = 3;
state_cov_size_ = 3;
- dim_ = 2;
}else if (_frame_structure == "PO" and _dim == 3)
{
state_size_ = 7;
state_cov_size_ = 6;
- dim_ = 3;
- } else if (_frame_structure == "POV" and _dim == 3)
+ }else if (_frame_structure == "POV" and _dim == 3)
{
state_size_ = 10;
state_cov_size_ = 9;
- dim_ = 3;
+ }else if (_frame_structure == "POVCDL" and _dim == 3)
+ {
+ state_size_ = 19;
+ state_cov_size_ = 18;
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
@@ -231,7 +237,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
- assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension");
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -240,10 +246,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
-
return prc_ptr;
}
@@ -279,7 +281,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
- assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension");
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
@@ -289,10 +291,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
return prc_ptr;
}
@@ -311,47 +309,6 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
-{
- for (auto sen : getHardware()->getSensorList()) // loop all sensors
- {
- auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
- sen->getProcessorList().end(),
- [&](ProcessorBasePtr prc)
- {
- return prc->getName() == _processor_name;
- }); // lambda function for the find_if
-
- if (prc_it != sen->getProcessorList().end()) // found something!
- {
- if ((*prc_it)->isMotion()) // found, and it's motion!
- {
- std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
- return processor_motion_ptr_;
- }
- else // found, but it's not motion!
- {
- std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
- return nullptr;
- }
- }
- }
- // nothing found!
- std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
- return nullptr;
-}
-
-void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
-{
- processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
- processor_motion_ptr_.reset();
-}
-
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
@@ -392,41 +349,108 @@ Eigen::VectorXd Problem::getCurrentState() const
void Problem::getCurrentState(Eigen::VectorXd& _state) const
{
- if (processor_motion_ptr_ != nullptr)
- processor_motion_ptr_->getCurrentState(_state);
- else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
- trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
- else
- _state = zeroState();
+ TimeStamp ts; // throwaway timestamp
+ getCurrentStateAndStamp(_state, ts);
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
-{
- if (processor_motion_ptr_ != nullptr)
+void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
+{
+ if (!processor_is_motion_list_.empty())
{
- processor_motion_ptr_->getCurrentState(_state);
- processor_motion_ptr_->getCurrentTimeStamp(ts);
+ // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
+ std::list<TimeStamp> proc_is_motion_current_ts;
+ for (auto proc: processor_is_motion_list_){
+ proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
+ }
+ auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
+ getState(*min_it, _state);
+ _ts = *min_it;
}
else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
{
- trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
+ // kind of redundant with getState(_ts, _state)
+ trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
}
else
_state = zeroState();
}
+
+// Problem of this implmentation: if more state
void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
{
- // try to get the state from processor_motion if any, otherwise...
- if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
- {
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
+ FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ if (processor_is_motion_list_.empty()){
if (closest_frame != nullptr)
- closest_frame->getState(_state);
+ _state = closest_frame->getState();
else
_state = zeroState();
}
+
+ // RETRIEVE FROM PROCESSOR MOTION
+ // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
+ else {
+ // Iterate over the problem state structure and get the corresponding state
+ // in the first processor is motion that provides it
+ // finally check if the state to concatenate list has the same total size as the problem state_size
+
+ // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
+ std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
+ for (auto proc: processor_is_motion_list_){
+ Eigen::VectorXd proc_state = proc->getState(_ts);
+
+ int idx = 0;
+ for (char sb_name: proc->getStateStructure()){
+ // not already there
+ if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
+ if (sb_name == 'O'){
+ int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
+ }
+ else{
+ int size_sb = dim_ == 3 ? 3 : 2;
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
+ }
+ }
+ }
+ }
+
+ int concat_size = 0;
+ for (auto state_map_it: states_to_concat_map){
+ concat_size += state_map_it.second.size();
+ }
+ // assert(concat_size == state_size_ && "Problem with the concatenated size: " );
+
+ // fill the state value from the state concatenation in the order dictated by frame_structure_
+ int idx = 0;
+ _state.resize(state_size_);
+ for (char sb_name: frame_structure_){
+ Eigen::VectorXd sb_state;
+ int size_sb; // really bad...
+ if (sb_name == 'O'){
+ size_sb = dim_ == 3 ? 4 : 1;
+ }
+ else {
+ size_sb = dim_ == 3 ? 3 : 2;
+ }
+ if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
+ sb_state = states_to_concat_map[sb_name];
+ }
+ else {
+ // Should be taken from the last state but too messy already
+ sb_state.resize(size_sb);
+ sb_state.setZero();
+ }
+
+ _state.segment(idx, size_sb) = sb_state;
+ idx += size_sb;
+
+ }
+ }
}
Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
@@ -462,6 +486,20 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
tree_manager_->setProblem(nullptr);
tree_manager_ = _gm;
tree_manager_->setProblem(shared_from_this());
+
+}
+
+void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
+{
+ processor_is_motion_list_.push_back(_processor_motion_ptr);
+}
+
+void Problem::clearProcessorIsMotion(IsMotionPtr proc)
+{
+ auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc);
+ if (it != processor_is_motion_list_.end()){
+ processor_is_motion_list_.erase(it);
+ }
}
Eigen::VectorXd Problem::zeroState() const
@@ -863,10 +901,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
// create origin capture with the given state as data
// Capture fix only takes 3d position and Quaternion orientation
CapturePosePtr init_capture;
- if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+ // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+ // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+ // else
+ // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+
+ if (this->getDim() == 3)
init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+ init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+
// emplace feature and factor
init_capture->emplaceFeatureAndFactor();
@@ -908,218 +952,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 (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
@@ -1128,236 +1180,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;
}
}
}
@@ -1365,361 +1389,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();
-
- auto Fo = c->getFrameOther();
- auto Co = c->getCaptureOther();
- auto fo = c->getFeatureOther();
- auto Lo = c->getLandmarkOther();
+ if (_verbose)
+ _stream << " Fac" << c->id() << " @ " << c.get();
- 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();
- 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);
+ if (_verbose)
+ {
+ _stream << " ( --> C" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // 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);
}
}
}
@@ -1729,98 +1775,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/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index a35097f2c70d3b047117ae6d0c9bf81a5fe2f5da..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -53,12 +53,13 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
- // if trigger, process directly without buffering
- if (triggerInCapture(_capture_ptr))
- processCapture(_capture_ptr);
// asking if capture should be stored
if (storeCapture(_capture_ptr))
buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+
+ // if trigger, process directly without buffering
+ if (triggerInCapture(_capture_ptr))
+ processCapture(_capture_ptr);
}
void ProcessorBase::remove()
@@ -68,12 +69,12 @@ void ProcessorBase::remove()
is_removing_ = true;
ProcessorBasePtr this_p = shared_from_this();
- // clear Problem::processor_motion_ptr_
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
- P->clearProcessorMotion();
+ auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
+ if(P && this_proc_cast_attempt )
+ P->clearProcessorIsMotion(this_proc_cast_attempt);
}
// remove from upstream
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 634d1f6d1284997573ef763bd329dcdf37cc94d3..aded1532cc17fc6a0f2e0d48edbbca857a1ab28b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -7,6 +7,7 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
@@ -25,7 +26,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
- dt_(0.0), x_(_state_size),
+ dt_(0.0),
+ x_(_state_size),
delta_(_delta_size),
delta_cov_(_delta_cov_size, _delta_cov_size),
delta_integrated_(_delta_size),
@@ -34,7 +36,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_cov_size_, calib_size_)
-{
+{
+ setStateStructure(_state_structure);
//
}
@@ -267,7 +270,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getCurrentState());
+ last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -315,7 +318,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new frame
auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getCurrentState(),
+ getProblem()->getCurrentState(),
getCurrentTimeStamp());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
@@ -346,6 +349,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
+// _x needs to have the size of the processor state
bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
CaptureMotionPtr capture_motion;
@@ -360,7 +364,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
// Get origin state and calibration
CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- VectorXd state_0 = cap_orig->getFrame()->getState();
+ VectorXd state_0 = cap_orig->getFrame()->getState(state_structure_);
VectorXd calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
@@ -371,7 +375,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ _x.resize( state_0.size() );
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
@@ -380,7 +384,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
else
{
// We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
+ WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
return false;
}
@@ -415,9 +419,11 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- _origin_frame->getTimeStamp());
+ _origin_frame->getState(),
+ origin_ts);
+
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
@@ -562,7 +568,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensor());
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
@@ -621,6 +628,9 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
if (_problem == nullptr or _problem == this->getProblem())
return;
@@ -629,11 +639,10 @@ void ProcessorMotion::setProblem(ProblemPtr _problem)
// set the origin
if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
this->setOrigin(this->getProblem()->getLastKeyFrame());
-
- // set the main processor motion
- if (this->getProblem()->getProcessorMotion() == nullptr)
- this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
-};
+
+ // adding processor is motion to the processor is motion vector
+ getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
+}
bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
{
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index c986ebf7cf9cd7fb83df1417ea19c42beed66c74..929c1ed7ceb825fdaa265a1af592fa8125832789 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -6,7 +6,7 @@ namespace wolf
{
ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
- ProcessorMotion("ProcessorOdom2d", 2, 3, 3, 3, 2, 0, _params),
+ ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 967031e9ccd1a68fcee41567be0f7dbf7523ef2c..3e1268eb1d705b3a335d9c8d9389e0c9449ebba1 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
- ProcessorMotion("ProcessorOdom3d", 3, 7, 7, 6, 6, 0, _params),
+ ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
params_odom_3d_ (_params),
k_disp_to_disp_ (0),
k_disp_to_rot_ (0),
diff --git a/src/utils/check_log.cpp b/src/utils/check_log.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06e58733aa3d99b9cffa312a9a88bdb223d15eb5
--- /dev/null
+++ b/src/utils/check_log.cpp
@@ -0,0 +1,29 @@
+#include "core/utils/check_log.h"
+
+using namespace wolf;
+
+CheckLog::CheckLog()
+{
+ is_consistent_ = true;
+ explanation_ = "";
+}
+CheckLog::CheckLog(bool _consistent, std::string _explanation)
+{
+ is_consistent_ = _consistent;
+ if (not _consistent)
+ explanation_ = _explanation;
+ else
+ explanation_ = "";
+}
+void CheckLog::compose(CheckLog l)
+{
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+}
+void CheckLog::assertTrue(bool _condition, std::stringstream& _stream)
+{
+ auto cl = CheckLog(_condition, _stream.str());
+ this->compose(cl);
+ // Clear inconsistency_explanation
+ std::stringstream().swap(_stream);
+}
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f
--- /dev/null
+++ b/src/utils/loader.cpp
@@ -0,0 +1,37 @@
+#include "core/utils/loader.h"
+
+#include <dlfcn.h>
+#include <stdexcept>
+
+Loader::Loader(std::string _file)
+{
+ path_ = _file;
+}
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+{
+ //
+}
+LoaderRaw::~LoaderRaw()
+{
+ close();
+}
+void LoaderRaw::load()
+{
+ resource_ = dlopen(path_.c_str(), RTLD_LAZY);
+ if (not resource_)
+ throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror());
+}
+void LoaderRaw::close()
+{
+ if (resource_)
+ dlclose(resource_);
+}
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// resource_ = new ClassLoader(path_);
+// }
+// void close(){
+// delete resource_;
+// }
+// };
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e
--- /dev/null
+++ b/src/utils/params_server.cpp
@@ -0,0 +1,28 @@
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+
+ParamsServer::ParamsServer()
+{
+ params_ = std::map<std::string, std::string>();
+}
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
+{
+ params_ = _params;
+}
+
+void ParamsServer::print()
+{
+ for (auto it : params_)
+ std::cout << it.first << "~~" << it.second << std::endl;
+}
+
+void ParamsServer::addParam(std::string _key, std::string _value)
+{
+ params_.insert(std::pair<std::string, std::string>(_key, _value));
+}
+
+void ParamsServer::addParams(std::map<std::string, std::string> _params)
+{
+ params_.insert(_params.begin(), _params.end());
+}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..267d0c9da2024abd59ed5b529d13de7680d14472
--- /dev/null
+++ b/src/yaml/parser_yaml.cpp
@@ -0,0 +1,547 @@
+#include "core/yaml/parser_yaml.h"
+
+#include <string>
+#include <vector>
+#include <list>
+#include <stack>
+#include <regex>
+#include <map>
+#include <iostream>
+#include <algorithm>
+#include <numeric>
+
+using namespace wolf;
+namespace {
+ //====== START OF FORWARD DECLARATION ========
+ std::string parseAtomicNode(YAML::Node);
+ std::string fetchMapEntry(YAML::Node);
+ std::string mapToString(std::map<std::string,std::string>);
+ //====== END OF FORWARD DECLARATION ========
+
+/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
+ * @param n the node representing a map
+ * @return std::map<std::string, std::string> populated with the key,value pairs in n
+ */
+std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
+ assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
+ auto m = std::map<std::string, std::string>();
+ for(const auto& kv : _n){
+ std::string key = kv.first.as<std::string>();
+ switch (kv.second.Type()) {
+ case YAML::NodeType::Scalar : {
+ std::string value = kv.second.Scalar();
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ case YAML::NodeType::Sequence : {
+ std::string aux = parseAtomicNode(kv.second);
+ m.insert(std::pair<std::string,std::string>(key, aux));
+ break;
+ }
+ case YAML::NodeType::Map : {
+ std::string value = fetchMapEntry(kv.second);
+ std::regex r("^\\$.*");
+ if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ default:
+ assert(1 == 0 && "Unsupported node Type at fetchAsMap");
+ break;
+ }
+ }
+ return m;
+}
+ std::string fetchMapEntry(YAML::Node _n){
+ switch (_n.Type()) {
+ case YAML::NodeType::Scalar: {
+ return _n.Scalar();
+ break;
+ }
+ case YAML::NodeType::Sequence: {
+ return parseAtomicNode(_n);
+ break;
+ }
+ case YAML::NodeType::Map: {
+ return mapToString(fetchAsMap(_n));
+ break;
+ }
+ default: {
+ assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+ return "";
+ break;
+ }
+ }
+ }
+ /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
+ * @param map_ just a std::map<std::string,std::string>
+ * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
+ */
+ std::string mapToString(std::map<std::string,std::string> _map){
+ std::string result = "";
+ auto v = std::vector<std::string>();
+ std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
+ auto concat = [](std::string ac, std::string str)-> std::string {
+ return ac + str + ",";
+ };
+ std::string aux = "";
+ std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
+ if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
+ else accumulated = "";
+ return "[" + accumulated + "]";
+ }
+ /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
+ * @param n a vector of YAML::Node that represents a YAML::Sequence
+ * @return <b>{std::string}</b> representing the YAML sequence
+ */
+ std::string parseAtomicNode(YAML::Node _n){
+ std::string aux = "";
+ bool first = true;
+ std::string separator = "";
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return _n.Scalar();
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n){
+ aux += separator + parseAtomicNode(it);
+ if(first){
+ separator = ",";
+ first = false;
+ }
+ }
+ return "[" + aux + "]";
+ break;
+ case YAML::NodeType::Map:
+ return mapToString(fetchAsMap(_n));
+ break;
+ default:
+ return "";
+ break;
+ }
+ }
+
+ /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
+ * Scalar, Sequence or Map.
+ * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
+ * @param n node to be test for atomicity
+ */
+ bool isAtomic(std::string _key, YAML::Node _n){
+ assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
+ std::regex r("^\\$.*");
+ bool is_atomic = true;
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return true;
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n) {
+ switch(it.Type()){
+ case YAML::NodeType::Map:
+ for(const auto& kv : it){
+ is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
+ }
+ break;
+ default:
+ is_atomic = is_atomic and isAtomic("", it);
+ break;
+ }
+ }
+ return is_atomic;
+ break;
+ case YAML::NodeType::Map:
+ is_atomic = std::regex_match(_key, r);
+ return is_atomic;
+ break;
+ default:
+ throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
+ return false;
+ break;
+ }
+ return false;
+ }
+}
+ParserYAML::ParserYAML(std::string _file, std::string path_root, bool freely_parse)
+{
+ params_ = std::map<std::string, std::string>();
+ active_name_ = "";
+ paramsSens_ = std::vector<ParamsInitSensor>();
+ paramsProc_ = std::vector<ParamsInitProcessor>();
+ subscriber_managers_ = std::vector<SubscriberManager>();
+ publisher_managers_ = std::vector<PublisherManager>();
+ parsing_file_ = std::stack<std::string>();
+ file_ = _file;
+ if (path_root != "")
+ {
+ std::regex r(".*/ *$");
+ if (not std::regex_match(path_root, r))
+ path_root_ = path_root + "/";
+ else
+ path_root_ = path_root;
+ }
+ if (not freely_parse)
+ parse();
+ else
+ parse_freely();
+}
+
+std::string ParserYAML::generatePath(std::string _file)
+{
+ std::regex r("^/.*");
+ if (std::regex_match(_file, r))
+ {
+ return _file;
+ }
+ else
+ {
+ return path_root_ + _file;
+ }
+}
+YAML::Node ParserYAML::loadYAML(std::string _file)
+{
+ try
+ {
+ // std::cout << "Parsing " << generatePath(_file) << std::endl;
+ WOLF_INFO("Parsing file: ", generatePath(_file));
+ return YAML::LoadFile(generatePath(_file));
+ }
+ catch (YAML::BadFile& e)
+ {
+ throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
+ parsing_file_.top());
+ }
+}
+std::string ParserYAML::tagsToString(std::vector<std::string>& _tags)
+{
+ std::string hdr = "";
+ for (auto it : _tags)
+ {
+ hdr = hdr + "/" + it;
+ }
+ return hdr;
+}
+void ParserYAML::walkTree(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ std::vector<std::string> hdrs = std::vector<std::string>();
+ walkTreeR(n, hdrs, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, hdr);
+ parsing_file_.pop();
+}
+void ParserYAML::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
+{
+ switch (_n.Type())
+ {
+ case YAML::NodeType::Scalar:
+ {
+ std::regex r("^@.*");
+ if (std::regex_match(_n.Scalar(), r))
+ {
+ std::string str = _n.Scalar();
+ walkTree(str.substr(1, str.size() - 1), _tags, hdr);
+ }
+ else
+ {
+ insert_register(hdr, _n.Scalar());
+ }
+ break;
+ }
+ case YAML::NodeType::Sequence:
+ {
+ if (isAtomic("", _n))
+ {
+ insert_register(hdr, parseAtomicNode(_n));
+ }
+ else
+ {
+ for (const auto& kv : _n)
+ {
+ walkTreeR(kv, _tags, hdr);
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Map:
+ {
+ for (const auto& kv : _n)
+ {
+ if (isAtomic(kv.first.as<std::string>(), _n))
+ {
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
+ key = key.substr(1, key.size() - 1);
+ insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+ }
+ else
+ {
+ /*
+ If key=="follow" then the parser will assume that the value is a path and will parse
+ the (expected) yaml file at the specified path. Note that this does not increase the header depth.
+ The following example shows how the header remains unafected:
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - follow: "@some_path" |
+ - var: 1.2 |
+ Resulting map:
+ cov_det -> 1
+ my_value-> 23
+ var: 1.2
+ Instead of:
+ cov_det -> 1
+ follow/my_value-> 23
+ var: 1.2
+ Which would result from the following yaml files
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - $follow: "@some_path" |
+ - var: 1.2 |
+ */
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
+ std::regex rr("follow");
+ if (not std::regex_match(kv.first.as<std::string>(), rr))
+ {
+ _tags.push_back(kv.first.as<std::string>());
+ if (_tags.size() == 2)
+ updateActiveName(kv.first.as<std::string>());
+ walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
+ _tags.pop_back();
+ if (_tags.size() == 1)
+ updateActiveName("");
+ }
+ else
+ {
+ walkTree(kv.second.as<std::string>(), _tags, hdr);
+ }
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Null:
+ break;
+ default:
+ assert(1 == 0 && "Unsupported node Type at walkTreeR.");
+ break;
+ }
+}
+void ParserYAML::updateActiveName(std::string _tag)
+{
+ active_name_ = _tag;
+}
+void ParserYAML::parseFirstLevel(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+
+ YAML::Node n_config = n["config"];
+ // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+ if (n_config.Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
+ generatePath(_file) + " starts with 'config:'");
+ if (n_config["problem"].Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
+ generatePath(_file) + " has a 'problem' entry");
+ problem = n_config["problem"];
+ std::vector<std::map<std::string, std::string>> map_container;
+ try
+ {
+ for (const auto& kv : n_config["sensors"])
+ {
+ ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
+ paramsSens_.push_back(pSensor);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() } }));
+ }
+ insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
+ ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["processors"])
+ {
+ ParamsInitProcessor pProc = {
+ kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
+ };
+ paramsProc_.push_back(pProc);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("processors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
+ ". Please make sure that each processor has 'type', 'name', 'plugin' and "
+ "'sensor_name' entries.");
+ }
+ try
+ {
+ for (const auto& kv : n_config["ROS subscriber"])
+ {
+ SubscriberManager pSubscriberManager = {
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
+ };
+ subscriber_managers_.push_back(pSubscriberManager);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
+ ". Please make sure that each manager has 'package', 'type', 'topic' and "
+ "'sensor_name' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["ROS publisher"])
+ {
+ WOLF_DEBUG("WHAT");
+ PublisherManager pPublisherManager = {
+ kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ };
+ publisher_managers_.push_back(pPublisherManager);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "period", kv["period"].Scalar() } }));
+ }
+ insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
+ ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+ }
+}
+std::map<std::string, std::string> ParserYAML::getParams()
+{
+ std::map<std::string, std::string> rtn = params_;
+ return rtn;
+}
+void ParserYAML::parse()
+{
+ parsing_file_.push(generatePath(file_));
+ parseFirstLevel(file_);
+
+ if (problem.Type() != YAML::NodeType::Undefined)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(problem, tags, "problem");
+ }
+ for (auto it : paramsSens_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("sensor");
+ walkTreeR(it.n_, tags, "sensor/" + it.name_);
+ }
+ for (auto it : paramsProc_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("processor");
+ walkTreeR(it.n_, tags, "processor/" + it.name_);
+ }
+ std::list<std::string> plugins, packages;
+ for (const auto& it : paramsSens_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : paramsProc_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : subscriber_managers_)
+ packages.push_back(it.package_);
+ plugins.sort();
+ plugins.unique();
+ packages.sort();
+ packages.unique();
+ insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+ insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+ YAML::Node n;
+ n = loadYAML(generatePath(file_));
+
+ if (n.Type() == YAML::NodeType::Map)
+ {
+ for (auto it : n)
+ {
+ auto node = it.second;
+ // WOLF_INFO("WUT ", it.first);
+ std::vector<std::string> tags = std::vector<std::string>();
+ if (it.first.as<std::string>() != "config")
+ walkTreeR(node, tags, it.first.as<std::string>());
+ else
+ {
+ for (auto itt : node)
+ {
+ std::string node_key = itt.first.as<std::string>();
+ if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+ node_key != "ROS subscriber" and node_key != "ROS publisher")
+ {
+ walkTreeR(itt.second, tags, node_key);
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(n, tags, "");
+ }
+ parsing_file_.pop();
+}
+void ParserYAML::parse_freely()
+{
+ parsing_file_.push(generatePath(file_));
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(loadYAML(file_), tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::insert_register(std::string _key, std::string _value)
+{
+ if (_key.substr(0, 1) == "/")
+ _key = _key.substr(1, _key.size() - 1);
+ auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
+ if (not inserted_it.second)
+ WOLF_WARN("Skipping key '",
+ _key,
+ "' with value '",
+ _value,
+ "'. There already exists the register: (",
+ inserted_it.first->first,
+ ",",
+ inserted_it.first->second,
+ ")");
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 83e0aa1df36f4018463fc2bb16ac57f96e329def..501965cdff67102f807e482f87ca7eafb7109e54 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)
@@ -173,6 +169,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_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
- // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
- Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+ Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
@@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 1, 0, 1
);
@@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 2, 0, 2
);
@@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
1, 2, 1, 2
);
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_param_server.cpp b/test/gtest_param_server.cpp
index 18f6b1eded0955faf7c6b823e25bbde62894203d..1f1f468360e1aa5073606f8ea32e7d82cafbc438 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index c8702619bc5150eb688bcb7dccfa11f71ab909e5..de5164ec6a9db61ad8a0344e8e6ee6d6e52dec16 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,7 +1,7 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index e0ba9a42a17936b62ec329c1fb022052d5ae6c30..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;
@@ -94,7 +98,7 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO", 3);
// check motion processor is null
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// add a motion sensor and processor
auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
@@ -103,7 +107,7 @@ TEST(Problem, Processor)
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>());
// check motion processor IS NOT by emplace
- ASSERT_EQ(P->getProcessorMotion(), Pm);
+ ASSERT_EQ(P->getProcessorIsMotion(), Pm);
}
TEST(Problem, Installers)
@@ -118,16 +122,16 @@ TEST(Problem, Installers)
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotion());
+ ASSERT_FALSE(P->getProcessorIsMotion());
// install processor motion
ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorMotion());
+ ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
// check motion processor is correct
- ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
}
TEST(Problem, SetOrigin_PO_2d)
@@ -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);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1933da8599234e61c534e9f05008710ac4396661..c4a1c4b0470c92bf09df1a950a1c19e57d35642d 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -125,7 +125,7 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
proc_trk->captureCallback(capt_trk);
-// problem->print(4,1,1,0);
+ problem->print(4,1,1,0);
// Only odom creating KFs
ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 52f86aced4329f61c433942cc25fd06d3cbe6034..5799117ffa523a9cbf5132ed504fa1525001c0ae 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -3,7 +3,7 @@
#include "core/problem/problem.h"
#include "dummy/tree_manager_dummy.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 14b05976faa99160657c303e028c1f651872f7ac..ed3a8052bfb4a73b4395cc885904d8f752af91d2 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -3,7 +3,7 @@
#include "core/problem/problem.h"
#include "core/tree_manager/tree_manager_sliding_window.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_odom_3d.h"