From 5d5afc013036144e478f4d1813b99ac8ee09be25 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 11 Mar 2020 12:01:55 +0100
Subject: [PATCH] Rename Intrinsics -> ParamsSensor
---
include/IMU/sensor/sensor_IMU.h | 20 ++++++++++----------
src/sensor/sensor_IMU.cpp | 8 ++++----
src/yaml/sensor_IMU_yaml.cpp | 6 +++---
test/CMakeLists.txt | 1 +
test/gtest_feature_IMU.cpp | 2 +-
5 files changed, 19 insertions(+), 18 deletions(-)
diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h
index c5da4386a..c8e13a2d8 100644
--- a/include/IMU/sensor/sensor_IMU.h
+++ b/include/IMU/sensor/sensor_IMU.h
@@ -8,10 +8,10 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorIMU);
-struct IntrinsicsIMU : public IntrinsicsBase
+struct ParamsSensorIMU : public ParamsSensorBase
{
//noise std dev
double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
@@ -25,13 +25,13 @@ struct IntrinsicsIMU : public IntrinsicsBase
double ab_rate_stdev = 0.00001;
double wb_rate_stdev = 0.00001;
- virtual ~IntrinsicsIMU() = default;
- IntrinsicsIMU()
+ virtual ~ParamsSensorIMU() = default;
+ ParamsSensorIMU()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
- IntrinsicsIMU(std::string _unique_name, const ParamsServer& _server):
- IntrinsicsBase(_unique_name, _server)
+ ParamsSensorIMU(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
{
w_noise = _server.getParam<double>(_unique_name + "/w_noise");
a_noise = _server.getParam<double>(_unique_name + "/a_noise");
@@ -42,7 +42,7 @@ struct IntrinsicsIMU : public IntrinsicsBase
}
std::string print() const
{
- return "\n" + IntrinsicsBase::print() + "\n"
+ return "\n" + ParamsSensorBase::print() + "\n"
+ "w_noise: " + std::to_string(w_noise) + "\n"
+ "a_noise: " + std::to_string(a_noise) + "\n"
+ "ab_initial_stdev: " + std::to_string(ab_initial_stdev) + "\n"
@@ -69,8 +69,8 @@ class SensorIMU : public SensorBase
public:
- SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params);
- SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params);
+ SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params);
+ SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params);
double getGyroNoise() const;
double getAccelNoise() const;
@@ -82,7 +82,7 @@ class SensorIMU : public SensorBase
virtual ~SensorIMU();
public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorBasePtr _intrinsics = nullptr);
};
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index 393af0da7..1036b7821 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -4,13 +4,13 @@
namespace wolf {
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params) :
SensorIMU(_extrinsics, *_params)
{
//
}
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params) :
SensorBase("SensorIMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
a_noise(_params.a_noise),
w_noise(_params.w_noise),
@@ -29,12 +29,12 @@ SensorIMU::~SensorIMU()
// Define the factory method
SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
+ const ParamsSensorBasePtr _intrinsics)
{
// decode extrinsics vector
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
- IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
+ ParamsSensorIMUPtr params = std::static_pointer_cast<ParamsSensorIMU>(_intrinsics);
SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
sen->setName(_unique_name);
return sen;
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index ab67315b6..89d7c382f 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -21,7 +21,7 @@ namespace wolf
namespace
{
-static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
@@ -30,7 +30,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
YAML::Node variances = config["motion_variances"];
YAML::Node kf_vote = config["keyframe_vote"];
- IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
+ ParamsSensorIMUPtr params = std::make_shared<ParamsSensorIMU>();
params->a_noise = variances["a_noise"] .as<double>();
params->w_noise = variances["w_noise"] .as<double>();
@@ -47,7 +47,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = IntrinsicsFactory::get().registerCreator("SensorIMU", createIntrinsicsIMU);
+const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorIMU", createParamsSensorIMU);
} // namespace [unnamed]
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 266bdfb1e..a6a1d9cd7 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -14,6 +14,7 @@ target_link_libraries(gtest_example ${PLUGIN_NAME}) #
###########################################################
wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
+message("WHAT IS HERE ${PLUGIN_NAME} ${wolf_LIBRARY}")
target_link_libraries(gtest_processor_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 73d5145e8..b994aac26 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -37,7 +37,7 @@ class FeatureIMU_test : public testing::Test
problem = Problem::create("POV", 3);
Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
+ ParamsSensorIMUPtr sen_imu_params = std::make_shared<ParamsSensorIMU>();
SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", IMU_extrinsics, sen_imu_params);
ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
prc_imu_params->max_time_span = 0.5;
--
GitLab