diff --git a/CMakeLists.txt b/CMakeLists.txt
index bf170ffeb867ac736bb4b83790e51ffedd40ba54..67fb89858a7c6c26a261477043c21c45334d1712 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -181,13 +181,14 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_UTILS
+ include/core/utils/check_log.hpp
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
@@ -286,7 +287,7 @@ SET(HDRS_SOLVER
)
SET(HDRS_YAML
- include/core/yaml/parser_yaml.hpp
+ include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
)
#SOURCES
@@ -320,6 +321,8 @@ SET(SRCS_MATH
)
SET(SRCS_UTILS
src/utils/converter_utils.cpp
+ src/utils/params_server.cpp
+ src/utils/loader.cpp
)
SET(SRCS_CAPTURE
@@ -368,6 +371,7 @@ SET(SRCS_SOLVER
src/solver/solver_manager.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..70fee2c15f074e571fd273d7cfccfbdc21c20552 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -101,7 +101,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);
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/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/problem/problem.h b/include/core/problem/problem.h
index 4f521c1155b33c1ab5afbf3aab67a3b38312dfd5..f2b19ef05c22f7a4c6777f889031dbda45d80a5b 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"
@@ -46,8 +46,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_;
@@ -147,15 +146,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, //
@@ -271,10 +270,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);
@@ -330,6 +329,11 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
// Print and check ---------------------------------------
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
@@ -345,11 +349,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false) const;
- std::string printToString(int depth = 4, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false) const;
bool check(int verbose_level = 0) const;
+ bool check(bool verbose, std::ostream& stream) const;
};
@@ -365,9 +366,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()
@@ -394,7 +408,8 @@ inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
return factor_notification_map_.size();
}
-
} // namespace wolf
+
+
#endif // PROBLEM_H
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 d0a250aa1012683bb9a38cce6cd2c8ae495c477a..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
*
*/
@@ -261,10 +266,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int id() const;
protected:
- /**
- * \brief set the pointer to problem
- */
- virtual void setProblem(ProblemPtr) override;
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
@@ -354,6 +355,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setVotingActive(bool _voting_active = true);
+ int getDim() const;
+
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
@@ -400,6 +403,10 @@ inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
{
params_->time_tolerance = _time_tolerance;
}
+inline int ProcessorBase::getDim() const
+{
+ return dim_;
+}
template<typename classType, typename... T>
std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
@@ -501,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_;
}
@@ -531,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..97c570cb8550d5fb9aa6a2bf076e7e9674ab75c5 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:
@@ -62,11 +66,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,60 +204,100 @@ 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 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() const
+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
+inline unsigned int HasStateBlocks::getLocalSize(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->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;
}
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ee5ae3cc972486d5567fa87684910cde74ab47fa..94def7273a8abed932f7dc25f0299cb282d5e624 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -293,6 +293,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.hpp b/include/core/utils/check_log.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..db3ab2dc9dd6e165c3541544bf7770022568f2fe
--- /dev/null
+++ b/include/core/utils/check_log.hpp
@@ -0,0 +1,32 @@
+#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/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/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 34e36705f665fa0d09cf6080b6f17697d9d58d55..42a11b3782a5a669264471bcae49571ec1322840 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");
@@ -140,7 +156,8 @@ void FrameBase::setNonEstimated()
{
// unregister if previously estimated
if (isKeyOrAux())
- removeStateBlocks(getProblem());
+ for (const auto& sb : getStateBlockVec())
+ getProblem()->notifyStateBlock(sb, REMOVE);
type_ = NON_ESTIMATED;
if (getTrajectory())
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 855236ece07f90a1570d293ac5c9333ee445658c..e79de9572f545e85142450497f3d4780621adc7b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -12,8 +12,9 @@
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
#include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
-#include "core/utils/loader.hpp"
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
+#include "core/utils/check_log.hpp"
// IRI libs includes
@@ -34,25 +35,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.");
@@ -183,7 +186,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
+
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
@@ -220,6 +224,12 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
+
+ //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");
+
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -227,10 +237,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;
}
@@ -262,17 +268,20 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
+
+ //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");
+
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
+
// setting the origin in all processor motion if origin already setted
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;
}
@@ -291,47 +300,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, //
@@ -372,41 +340,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
@@ -436,6 +471,18 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
+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
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
@@ -831,10 +878,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();
@@ -872,55 +925,53 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, 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
{
- using std::cout;
- using std::endl;
- cout << endl;
- cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
+ 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())
{
- cout << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
if (depth < 2)
- cout << " -- " << S->getProcessorList().size() << "p";
- cout << endl;
+ stream << " -- " << S->getProcessorList().size() << "p";
+ stream << std::endl;
if (metric && state_blocks)
{
- cout << " sb: ";
+ stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << 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() << " ); ";
}
- cout << endl;
+ stream << std::endl;
}
else if (metric)
{
- cout << " ( ";
+ stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << sb->getState().transpose() << " ";
+ stream << sb->getState().transpose() << " ";
}
- cout << ")" << endl;
+ stream << ")" << std::endl;
}
else if (state_blocks)
{
- cout << " sb: ";
+ stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
- cout << endl;
+ stream << std::endl;
}
if (depth >= 2)
{
@@ -929,126 +980,135 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
{
if (p->isMotion())
{
- std::cout << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
+ stream << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ << pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pm->getLast()->getFrame()->id() << endl;
+ stream << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
- cout << " i: C" << pm->getIncoming()->id() << endl;
+ stream << " i: C" << pm->getIncoming()->id() << std::endl;
}
else
{
- cout << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
+ stream << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getLast()->getFrame()->id() << endl;
+ stream << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
- cout << " i: C" << pt->getIncoming()->id() << endl;
+ stream << " i: C" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
+ stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
if (depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
{
- cout << " <-- ";
+ stream << " <-- ";
for (auto cby : F->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
{
- cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp();
- cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- cout << endl;
+ stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ stream << std::endl;
}
if (state_blocks)
{
- cout << " sb:";
+ stream << " sb:";
for (const auto& sb : F->getStateBlockVec())
{
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- cout << endl;
+ stream << std::endl;
}
+ // if (state_blocks)
+ // {
+ // cout << " sb:";
+ // for (const auto& sb : F->getStateBlockVec())
+ // {
+ // cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ // }
+ // cout << endl;
+ // }
if (depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- cout << " -> S" << C->getSensor()->id();
+ stream << " -> Sen" << C->getSensor()->id();
}
else
- cout << " -> S-";
+ stream << " -> S-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
- cout << " -> OC" << OC->id();
+ stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- cout << " ; OF" << OF->id();
+ stream << " ; OFrm" << OF->id();
}
}
- cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
if (constr_by)
{
- cout << " <-- ";
+ stream << " <-- ";
for (auto cby : C->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
- cout << " sb: ";
- cout << (sb->isFixed() ? "Fix" : "Est");
+ stream << " sb: ";
+ stream << (sb->isFixed() ? "Fix" : "Est");
if (metric)
- cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- cout << endl;
+ stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- cout << " buffer size : " << CM->getBuffer().get().size() << endl;
+ stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
if ( metric && ! CM->getBuffer().get().empty())
{
- cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+ stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
- cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << 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;
}
}
}
@@ -1058,34 +1118,34 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
if (constr_by)
{
- cout << " <--\t";
+ stream << " <--\t";
for (auto cby : f->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
- cout << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
+ stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ << " )" << std::endl;
if (depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- cout << " c" << c->id() << " " << c->getType() << " -->";
+ stream << " Fac" << c->id() << " " << c->getType() << " -->";
if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- cout << " A";
+ stream << " Abs";
if (c->getFrameOther() != nullptr)
- cout << " F" << c->getFrameOther()->id();
+ stream << " Frm" << c->getFrameOther()->id();
if (c->getCaptureOther() != nullptr)
- cout << " C" << c->getCaptureOther()->id();
+ stream << " Cap" << c->getCaptureOther()->id();
if (c->getFeatureOther() != nullptr)
- cout << " f" << c->getFeatureOther()->id();
+ stream << " Fac" << c->getFeatureOther()->id();
if (c->getLandmarkOther() != nullptr)
- cout << " L" << c->getLandmarkOther()->id();
- cout << endl;
+ stream << " Lmk" << c->getLandmarkOther()->id();
+ stream << std::endl;
} // for c
}
} // for f
@@ -1094,495 +1154,332 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
}
} // for F
}
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
+ stream << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
if (depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
- cout << " L" << L->id() << " " << L->getType();
+ stream << " Lmk" << L->id() << " " << L->getType();
if (constr_by)
{
- cout << "\t<-- ";
+ stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
{
- cout << (L->isFixed() ? " Fix" : " Est");
- cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- cout << endl;
+ stream << (L->isFixed() ? " Fix" : " Est");
+ stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ stream << std::endl;
}
if (state_blocks)
{
- cout << " sb:";
+ stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
- cout << (sb->isFixed() ? " Fix" : " Est");
+ stream << (sb->isFixed() ? " Fix" : " Est");
}
- cout << endl;
+ stream << std::endl;
}
} // for L
}
- cout << "-----------------------------------------" << endl;
- cout << endl;
+ stream << "-----------------------------------------" << std::endl;
+ stream << std::endl;
}
-std::string Problem::printToString(int depth, bool constr_by, bool metric, bool state_blocks) const
+bool Problem::check(bool verbose, std::ostream& stream) const
{
- using std::cout;
- using std::endl;
- std::stringstream result;
-
- result << endl;
- result << "P: wolf tree status ---------------------" << endl;
- result << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
- if (depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- result << " -- " << S->getProcessorList().size() << "p";
- result << endl;
- if (metric && state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- }
- result << endl;
- }
- else if (metric)
- {
- result << " ( ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << sb->getState().transpose() << " ";
- }
- result << ")" << endl;
- }
- else if (state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- }
- result << endl;
- }
+ CheckLog log(true, "");
+ log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- if (depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- result << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- result << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
- if (pm->getLast())
- result << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pm->getLast()->getFrame()->id() << endl;
- if (pm->getIncoming())
- result << " i: C" << pm->getIncoming()->id() << endl;
- }
- else
- {
- result << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
- if (pt->getOrigin())
- result << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
- if (pt->getLast())
- result << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getLast()->getFrame()->id() << endl;
- if (pt->getIncoming())
- result << " i: C" << pt->getIncoming()->id() << endl;
- }
- }
- } // for p
- }
- } // for S
- }
- result << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
- if (depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- result << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : F->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp().get();
- result << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : F->getStateBlockVec())
- if (sb != nullptr)
- result << " " << (sb->isFixed() ? "Fix" : "Est");
- result << endl;
- }
- if (depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- result << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
-
- if(C->getSensor() != nullptr)
- {
- result << " -> S" << C->getSensor()->id();
- }
- else
- result << " -> S-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (auto OC = CM->getOriginCapture())
- {
- result << " -> OC" << OC->id();
- if (auto OF = OC->getFrame())
- result << " ; OF" << OF->id();
- }
- }
-
- result << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : C->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
-
- if (state_blocks)
- for(auto sb : C->getStateBlockVec())
- if(sb != nullptr)
- {
- result << " sb: ";
- result << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- result << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- result << endl;
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- result << " buffer size : " << CM->getBuffer().get().size() << endl;
- if ( metric && ! CM->getBuffer().get().empty())
- {
- result << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
- if (CM->hasCalibration())
- {
- result << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- result << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- result << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- result << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
- }
- }
- }
-
- if (depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- result << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
- {
- result << " <--\t";
- for (auto cby : f->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- result << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
- if (depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- result << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- result << " A";
- if (c->getFrameOther() != nullptr)
- result << " F" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- result << " C" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- result << " f" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- result << " L" << c->getLandmarkOther()->id();
- result << endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- result << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
- if (depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- result << " L" << L->id() << " " << L->getType();
- if (constr_by)
- {
- result << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (L->isFixed() ? " Fix" : " Est");
- result << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : L->getStateBlockVec())
- if (sb != nullptr)
- result << (sb->isFixed() ? " Fix" : " Est");
- result << endl;
- }
- } // for L
- }
- result << "-----------------------------------------" << endl;
- result << endl;
- return result.str();
-}
-
-void Problem::perturb(double amplitude)
-{
- // Sensors
- for (auto& S : getHardware()->getSensorList())
- S->perturb(amplitude);
-
- // Frames and Captures
- for (auto& F : getTrajectory()->getFrameList())
- {
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
- C->perturb(amplitude);
- }
-
- // Landmarks
- for (auto& L : getMap()->getLandmarkList())
- L->perturb(amplitude);
-}
-
-bool Problem::check(int verbose_level) const
-{
- using std::cout;
- using std::endl;
-
- bool is_consistent = true; // true if all checks passed; false if any check fails.
-
- if (verbose_level) cout << endl;
- if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
+ if (verbose) stream << std::endl;
+ if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
auto P_raw = this;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "P @ " << P_raw << endl;
+ stream << "Prb @ " << P_raw << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "H @ " << H.get() << endl;
+ stream << "Hrw @ " << H.get() << std::endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+ 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);
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " S" << S->id() << " @ " << S.get() << endl;
- cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardware().get() << endl;
- for (auto sb : S->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
// check problem and hardware pointers
- is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardware() == H);
-
- // Processors =======================================================================================
+ // 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);
+
+ // 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);
+
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
- cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << 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);
- is_consistent = is_consistent && (p->getSensor() == S);
+
+ // is_consistent = is_consistent && (p->getProblem().get() == P_raw);
+
+ 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);
+
+ // 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);
}
}
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "T @ " << T.get() << endl;
+ stream << "Trj @ " << T.get() << std::endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+
+ 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);
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose_level > 0)
- {
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl;
- cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectory().get() << endl;
- for (const auto& sb : F->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
+ 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;
+ }
+ for (const auto &pair: F->getStateBlockMap()) {
+ auto sb = pair.second;
+ inconsistency_explanation << "Frame's " << 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();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
}
- }
+ stream << std::endl;
+ }
+ }
+
// check problem and trajectory pointers
- is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectory() == T);
+ // is_consistent = is_consistent && (F->getProblem().get() == P_raw);
+
+ 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);
+
+ 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);
+
for (auto cby : F->getConstrainedByList())
{
- if (verbose_level > 0)
- {
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
- }
+ 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);
+ // is_consistent = is_consistent && (cby->getFrameOther() == F);
+
+ 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);
+
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
+
+ if (verbose) {
+ stream << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
}
+ stream << std::endl;
+ }
}
}
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensor()) cout << C->getSensor()->id();
- else cout << "-";
- cout << endl;
- cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
- for (auto sb : C->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
- if (sb)
+ auto sb = pair.second;
+ if (verbose)
{
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
+ }
+ stream << std::endl;
}
- cout << endl;
}
- }
+
// check problem and frame pointers
- is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFrame() == F);
+ // 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);
+
+ // 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);
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " f" << f->id() << " @ " << f.get() << endl;
- cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << endl;
+ 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);
- is_consistent = is_consistent && (f->getCapture() == C);
+ // is_consistent = is_consistent && (f->getProblem().get() == P_raw);
+
+ 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);
+
+ // 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);
for (auto cby : f->getConstrainedByList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
+ stream << " <- c" << cby->id() << " -> Ftr" << cby->getFeatureOther()->id() << std::endl;
}
// check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOther() == f);
+ // is_consistent = is_consistent && (cby->getFeatureOther() == f);
+
+ 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_level > 0)
- cout << " c" << c->id() << " @ " << c.get();
+ if (verbose)
+ stream << " Fac" << c->id() << " @ " << c.get();
auto Fo = c->getFrameOther();
auto Co = c->getCaptureOther();
@@ -1591,147 +1488,264 @@ bool Problem::check(int verbose_level) const
if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
{
- if (verbose_level > 0)
- cout << " --> Abs." << endl;
+ if (verbose)
+ stream << " --> Abs." << std::endl;
}
// find constrained_by pointer in constrained frame
if ( Fo ) // case FRAME:
{
- if (verbose_level > 0)
- cout << " --> F" << Fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Frm" << Fo->id() << " <- ";
bool found = false;
for (auto cby : Fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << endl;
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
+ // 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);
+
}
// find constrained_by pointer in constrained capture
if ( Co ) // case CAPTURE:
{
- if (verbose_level > 0)
- cout << " --> C" << Co->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Cap" << Co->id() << " <- ";
bool found = false;
for (auto cby : Co->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << endl;
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained frame
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
+
+ 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);
+
}
// find constrained_by pointer in constrained feature
if ( fo ) // case FEATURE:
{
- if (verbose_level > 0)
- cout << " --> f" << fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Ftr" << fo->id() << " <- ";
bool found = false;
for (auto cby : fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << endl;
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained feature
- is_consistent = is_consistent && found;
+ // 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);
}
// find constrained_by pointer in constrained landmark
if ( Lo ) // case LANDMARK:
{
- if (verbose_level > 0)
- cout << " --> L" << Lo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Lmk" << Lo->id() << " <- ";
bool found = false;
for (auto cby : Lo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << endl;
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained landmark
- is_consistent = is_consistent && found;
+ // 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_level > 0)
+ if (verbose)
+ stream << std::endl;
+
+ if (verbose)
{
- cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << 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);
- is_consistent = is_consistent && (c->getFeature() == f);
+ // 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);
+
+ // is_consistent = is_consistent && (c->getFeature() == f);
+
+ inconsistency_explanation << "The factor " << 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);
// 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_level > 0)
+ if (verbose)
{
- cout << " sb @ " << sb.get();
+ stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
}
+ bool found_here;
+ std::vector<StateBlockPtr> sb_vec;
+
// find in own Frame
- found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end());
+ 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 = found || found_here;
+
// find in own Capture
- found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
+ 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 = 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 = found || found_here;
+ }
+
// find in own Sensor
if (S)
{
- auto sb_vec = S->getStateBlockVec();
- found = found || (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ 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 = found || found_here;
}
+
// find in constrained Frame
- if (Fo)
- found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end());
+ 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;
+
+ }
+
// find in constrained Capture
if (Co)
- found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
+ {
+ 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())
+ {
+ 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();
+ found = found || found_here;
+ }
+
// find in constrained Feature
if (fo)
{
// find in constrained feature's Frame
- FrameBasePtr foF = fo->getFrame();
- found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end());
+ 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;
+
// find in constrained feature's Capture
- CaptureBasePtr foC = fo->getCapture();
- found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
+ 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 Sensor
- SensorBasePtr foS = fo->getCapture()->getSensor();
- found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
+ 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 landmark
if (Lo)
- found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
- if (verbose_level > 0)
+ {
+ 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 (verbose)
{
if (found)
- cout << " found";
+ stream << " found";
else
- cout << " NOT FOUND !";
- cout << endl;
+ stream << " NOT FOUND !";
+ stream << std::endl;
}
+
+ 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);
+
// check that all state block pointers were found
- is_consistent = is_consistent && found;
+ // is_consistent = is_consistent && found;
}
}
}
@@ -1741,63 +1755,103 @@ bool Problem::check(int verbose_level) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose_level > 0)
- cout << "M @ " << M.get() << endl;
+ if (verbose)
+ stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
- is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+ 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);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " L" << L->id() << " @ " << L.get() << endl;
- cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMap().get() << endl;
- for (const auto& sb : L->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
// check problem and map pointers
- is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMap() == M);
- for (auto cby : L->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
- // check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
+ // is_consistent = is_consistent && (L->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "The 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);
+
+ // is_consistent = is_consistent && (L->getMap() == M);
+
+ 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);
+
+ 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;
}
+ }
}
}
- if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl;
- if (verbose_level) cout << 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 is_consistent;
+ return log.is_consistent_;
}
+bool Problem::check(int verbose_level) const
+{
+ return check((verbose_level > 0), std::cout);
+}
+void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
+{
+ print(depth, std::cout, constr_by, metric, state_blocks);
+}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
if (depth.compare("T") == 0)
@@ -1814,4 +1868,26 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
print(0, constr_by, metric, state_blocks);
}
+void Problem::perturb(double amplitude)
+{
+ // Sensors
+ for (auto& S : getHardware()->getSensorList())
+ S->perturb(amplitude);
+
+ // Frames and Captures
+ for (auto& F : getTrajectory()->getFrameList())
+ {
+ if (F->isKeyOrAux())
+ {
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
+ }
+ }
+
+ // Landmarks
+ for (auto& L : getMap()->getLandmarkList())
+ L->perturb(amplitude);
+}
+
} // namespace wolf
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 954531df6d99ba8df5ccf310f3ae56cba5e9bc8a..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -22,13 +22,6 @@ ProcessorBase::~ProcessorBase()
// WOLF_DEBUG("destructed -p" , id());
}
-void ProcessorBase::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());
-
- NodeBase::setProblem(_problem);
-}
bool ProcessorBase::permittedKeyFrame()
{
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
@@ -60,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()
@@ -75,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 1818c2fbb57aabadd33af0203a4083483aa931bd..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,18 +364,18 @@ 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
VectorXd calib_preint = capture_motion->getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
-
+
VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
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,13 +628,10 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
- if (_problem == nullptr)
- return;
-
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());
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
- if (_problem == this->getProblem())
+ if (_problem == nullptr or _problem == this->getProblem())
return;
NodeBase::setProblem(_problem);
@@ -635,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/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/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_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..ae7e14ae893449e8c42df4736eae792dfbd4b20d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -94,7 +94,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 +103,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 +118,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)
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 );