diff --git a/CMakeLists.txt b/CMakeLists.txt
index c94b33bfb04c9751c727f62bb16557cd1ef8f508..723c125ca15bb64fa2f98d98d37183fd87125dcb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -194,30 +194,30 @@ SET(HDRS_PROBLEM
include/core/problem/problem.h
)
SET(HDRS_PROCESSOR
+ include/core/processor/factory_processor.h
include/core/processor/motion_buffer.h
include/core/processor/motion_provider.h
include/core/processor/processor_base.h
- include/core/processor/processor_diff_drive.h
- include/core/processor/processor_fixed_wing_model.h
- include/core/processor/factory_processor.h
- include/core/processor/processor_loop_closure.h
+ # include/core/processor/processor_diff_drive.h
+ # include/core/processor/processor_fixed_wing_model.h
+ # include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
- include/core/processor/processor_odom_2d.h
- include/core/processor/processor_odom_3d.h
- include/core/processor/processor_pose.h
+ # include/core/processor/processor_odom_2d.h
+ # include/core/processor/processor_odom_3d.h
+ # include/core/processor/processor_pose.h
include/core/processor/processor_tracker.h
include/core/processor/processor_tracker_feature.h
include/core/processor/processor_tracker_landmark.h
include/core/processor/track_matrix.h
)
SET(HDRS_SENSOR
- include/core/sensor/sensor_base.h
- include/core/sensor/sensor_diff_drive.h
include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_motion_model.h
- include/core/sensor/sensor_odom_2d.h
- include/core/sensor/sensor_odom_3d.h
- include/core/sensor/sensor_pose.h
+ include/core/sensor/sensor_base.h
+ # include/core/sensor/sensor_diff_drive.h
+ # include/core/sensor/sensor_motion_model.h
+ # include/core/sensor/sensor_odom_2d.h
+ # include/core/sensor/sensor_odom_3d.h
+ # include/core/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
@@ -230,6 +230,7 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_base.h
include/core/state_block/local_parametrization_homogeneous.h
include/core/state_block/local_parametrization_quaternion.h
+ include/core/state_block/prior.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
include/core/state_block/state_composite.h
@@ -306,13 +307,13 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
- src/processor/processor_diff_drive.cpp
- src/processor/processor_fixed_wing_model.cpp
- src/processor/processor_loop_closure.cpp
+ # src/processor/processor_diff_drive.cpp
+ # src/processor/processor_fixed_wing_model.cpp
+ # src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
- src/processor/processor_odom_2d.cpp
- src/processor/processor_odom_3d.cpp
- src/processor/processor_pose.cpp
+ # src/processor/processor_odom_2d.cpp
+ # src/processor/processor_odom_3d.cpp
+ # src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
@@ -320,11 +321,11 @@ SET(SRCS_PROCESSOR
)
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
- src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_motion_model.cpp
- src/sensor/sensor_odom_2d.cpp
- src/sensor/sensor_odom_3d.cpp
- src/sensor/sensor_pose.cpp
+ # src/sensor/sensor_diff_drive.cpp
+ # src/sensor/sensor_motion_model.cpp
+ # src/sensor/sensor_odom_2d.cpp
+ # src/sensor/sensor_odom_3d.cpp
+ # src/sensor/sensor_pose.cpp
)
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
@@ -353,10 +354,10 @@ SET(SRCS_UTILS
)
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
- src/yaml/sensor_pose_yaml.cpp
+ # src/yaml/processor_odom_3d_yaml.cpp
+ # src/yaml/sensor_odom_2d_yaml.cpp
+ # src/yaml/sensor_odom_3d_yaml.cpp
+ # src/yaml/sensor_pose_yaml.cpp
)
# ============ OPTIONALS ============
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index faf359f727df4058543204a9a2e29d661e4ca96d..984ec4fa52840ebffb139e8a256bf468146d7753 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -104,7 +104,7 @@ struct ParamsSensorBase: public ParamsBase
std::string print() const override
{
- return "noise_std: " + std::to_string(noise_std) + "\n";
+ return "noise_std: ";// + to_string(noise_std) + "\n";
}
};
@@ -128,7 +128,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
protected:
- //Eigen::VectorXd noise_std_; // std of sensor noise
+ Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
void setProblem(ProblemPtr _problem) override final;
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index b246ed47a1f99187ee27615269bbc7f681cadb97..14aba5e5fc0631b9fb6c30a32366fd79728a516b 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -24,6 +24,8 @@
#include "core/utils/params_server.h"
+using std::to_string;
+
namespace wolf
{
@@ -33,12 +35,12 @@ typedef std::unordered_map<char, Prior> Priors;
class Prior
{
private:
- char key; // State key
- std::string mode; // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor'
- Eigen::VectorXd state; // state values (only filled from server if mode != 'nothing')
- Eigen::VectorXd sigma; // factor sigmas (only filled from server if mode == 'factor')
- bool dynamic; // State dynamic
- Eigen::VectorXd sigma_drift; // drift of the state (only filled from server if dynamic)
+ char key_; // State key
+ std::string mode_; // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor'
+ Eigen::VectorXd state_; // state values (only filled from server if mode != 'nothing')
+ Eigen::VectorXd sigma_; // factor sigmas (only filled from server if mode == 'factor')
+ bool dynamic_; // State dynamic
+ Eigen::VectorXd sigma_drift_; // drift of the state (only filled from server if dynamic)
public:
Prior() = default;
@@ -55,69 +57,69 @@ class Prior
char getKey() const
{
- return key;
+ return key_;
}
const std::string& getMode() const
{
- return mode;
+ return mode_;
}
const Eigen::VectorXd& getState() const
{
- return state;
+ return state_;
}
const Eigen::VectorXd& getSigma() const
{
- return sigma;
+ return sigma_;
}
bool isDynamic() const
{
- return dynamic;
+ return dynamic_;
}
bool isFixed() const
{
- return mode == "fix";
+ return mode_ == "fix";
}
bool isInitialGuess() const
{
- return mode == "initial_guess";
+ return mode_ == "initial_guess";
}
bool isFactor() const
{
- return mode == "factor";
+ return mode_ == "factor";
}
- const Eigen::VectorXd& getSigma() const
+ const Eigen::VectorXd& getSigmaDrift() const
{
- return sigma_drift;
+ return sigma_drift_;
}
void check() const
{
- if (mode != "nothing" and mode != "initial_guess" and mode != "fix" and mode == "factor")
+ if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ == "factor")
throw std::runtime_error("wrong mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
- if ( mode == "nothing" and (key == 'P' or key == 'O'))
+ if ( mode_ == "nothing" and (key_ == 'P' or key_ == 'O'))
throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
- if (dynamic and (key == 'P' or key == 'O') )
+ if (dynamic_ and (key_ == 'P' or key_ == 'O') )
throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
}
virtual std::string print() const final
{
- return "Prior " + key + "\n"
- + "mode: " + std::to_string(mode) + "\n"
- + "state: " + std::to_string(state) + "\n"
- + (mode == "factor" ? "sigma: " + std::to_string(sigma) + "\n" : "")
- + "dynamic: " + std::to_string(dynamic) + "\n"
- + (dynamic ? "sigma_drift: "+ std::to_string(sigma_drift) + "\n" : "");
+ return "Prior " + std::string(1,key_) + "\n"
+ + "mode: " + mode_ + "\n"
+ //+ "state: " + to_string(state_) + "\n"
+ //+ (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "")
+ + "dynamic: " + to_string(dynamic_) + "\n";
+ //+ (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
}
};
@@ -127,36 +129,36 @@ inline Prior::Prior(char _key,
const Eigen::VectorXd& _sigma,
bool _dynamic,
const Eigen::VectorXd& _sigma_drift) :
- key(_key),
- mode(_mode),
- state(_state),
- sigma(_sigma),
- dynamic(_dynamic),
- sigma_drift(_sigma_drift)
+ key_(_key),
+ mode_(_mode),
+ state_(_state),
+ sigma_(_sigma),
+ dynamic_(_dynamic),
+ sigma_drift_(_sigma_drift)
{
check();
}
inline Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
{
- mode = _server.getParam<double>(_prefix + _key + "/mode");
+ mode_ = _server.getParam<double>(_prefix + _key + "/mode");
- if (mode != "nothing")
- state = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
+ if (mode_ != "nothing")
+ state_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
else
- state = Eigen::VectorXd(0);
+ state_ = Eigen::VectorXd(0);
- if (mode == "factor")
- sigma = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
+ if (mode_ == "factor")
+ sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
else
- sigma = Eigen::VectorXd(0);
+ sigma_ = Eigen::VectorXd(0);
- dynamic = _server.getParam<bool>(_prefix + _key + "/dynamic");
+ dynamic_ = _server.getParam<bool>(_prefix + _key + "/dynamic");
- if (dynamic)
- sigma_drift = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
+ if (dynamic_)
+ sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
else
- sigma_drift = Eigen::VectorXd(0);
+ sigma_drift_ = Eigen::VectorXd(0);
check();
}
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index e5283ccd5b5f669e000fda1904245c99e58955ee..661957f029005a979737b50cdc5add7fff44e6cf 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -71,6 +71,17 @@ public:
};
+// template<typename Derived>
+// std::string to_string(const Eigen::DenseBase<Derived>& mat)
+// {
+// std::stringstream ss;
+// if (mat.cols() == 1)
+// ss << mat.transpose();
+// else
+// ss << std::endl << mat;
+// return ss.str();
+// }
+
}
#endif
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 954af1f0de9042172c4e46802004adb7ef6bca8e..47b7685eca0237cb310b4526d8bf26691ab6fda4 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -223,12 +223,14 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
}
}
-void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) {
+void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std)
+{
noise_std_ = _noise_std;
noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
}
-void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
+void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov)
+{
noise_std_ = _noise_cov.diagonal().array().sqrt();
noise_cov_ = _noise_cov;
}
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index ce4a79743d24302907e85117b812458ed4de6c83..7b2aea7e370eb36c56a539261e3b914307e7e913 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -21,7 +21,8 @@
//--------LICENSE_END--------
#include "core/utils/params_server.h"
-using namespace wolf;
+namespace wolf
+{
ParamsServer::ParamsServer()
{
@@ -47,3 +48,4 @@ void ParamsServer::addParams(std::map<std::string, std::string> _params)
{
params_.insert(_params.begin(), _params.end());
}
+}
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index dd7732c47dbffb1e1e7e2502d0ecc78c60e9f926..4bf4cd05ee026638264f0584110d0e81038923be 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -244,12 +244,12 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# ProcessorFixedWingModel class test
-wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
-target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+# target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
# ProcessorDiffDrive class test
-wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+# target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
@@ -260,16 +260,16 @@ target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
# ProcessorMotion in 2d
-wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+# target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
# ProcessorOdom3d class test
-wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
+# target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
# FactorPose3dWithExtrinsics class test
-wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+# target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
# ProcessorTrackerFeatureDummy class test
@@ -281,12 +281,12 @@ wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_la
target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy)
# SensorDiffDriveSelfcalib class test
-wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
+# target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
# SensorDiffDriveSelfcalib class test
-wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
-target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+# target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
IF (Ceres_FOUND)
# SolverCeres test