diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
index 5c623b82d4879cf3d9888870ef05083e050fdb91..3eba9dc3e7e6c69841f4cc0cbac9674028c1fe44 100644
--- a/demos/processor_imu.yaml
+++ b/demos/processor_imu.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3d.yaml
similarity index 100%
rename from demos/processor_odom_3D.yaml
rename to demos/processor_odom_3d.yaml
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
index 941b84610845f31cfed813394d85c923d2a3211b..3c78a00d35c785fe73381d8f6ce99a705d27ce77 100644
--- a/demos/sensor_imu.yaml
+++ b/demos/sensor_imu.yaml
@@ -1,4 +1,4 @@
-type: "SensorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
motion_variances:
a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3d.yaml
similarity index 100%
rename from demos/sensor_odom_3D.yaml
rename to demos/sensor_odom_3d.yaml
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 0cb27344610485b91f45770b32370faa81d4ab8d..3d3a04390065ce0d523c78db0a27e6057437dd7e 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -7,12 +7,12 @@
#include "core/processor/processor_motion.h"
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsImu);
-struct ProcessorParamsIMU : public ProcessorParamsMotion
+struct ProcessorParamsImu : public ProcessorParamsMotion
{
- ProcessorParamsIMU() = default;
- ProcessorParamsIMU(std::string _unique_name, const ParamsServer& _server):
+ ProcessorParamsImu() = default;
+ ProcessorParamsImu(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsMotion(_unique_name, _server)
{
//
@@ -23,17 +23,17 @@ struct ProcessorParamsIMU : public ProcessorParamsMotion
}
};
-WOLF_PTR_TYPEDEFS(ProcessorIMU);
+WOLF_PTR_TYPEDEFS(ProcessorImu);
//class
-class ProcessorIMU : public ProcessorMotion{
+class ProcessorImu : public ProcessorMotion{
public:
- ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU);
- virtual ~ProcessorIMU();
+ ProcessorImu(ProcessorParamsImuPtr _params_motion_Imu);
+ virtual ~ProcessorImu();
// virtual void configure(SensorBasePtr _sensor) override { };
virtual void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorIMU, ProcessorParamsIMU);
+ WOLF_PROCESSOR_CREATE(ProcessorImu, ProcessorParamsImu);
protected:
virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -72,7 +72,7 @@ class ProcessorIMU : public ProcessorMotion{
CaptureBasePtr _capture_origin) override;
protected:
- ProcessorParamsIMUPtr params_motion_IMU_;
+ ProcessorParamsImuPtr params_motion_Imu_;
Eigen::Matrix<double, 9, 9> unmeasured_perturbation_cov_;
};
@@ -85,7 +85,7 @@ class ProcessorIMU : public ProcessorMotion{
namespace wolf{
-inline Eigen::VectorXd ProcessorIMU::deltaZero() const
+inline Eigen::VectorXd ProcessorImu::deltaZero() const
{
return (Eigen::VectorXd(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
}
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 17b451ff3fd4bb48e5d0de20df3b24e78f366011..b2cd4dbcae2b16949b52312f911e50e3f4cd3506 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(ParamsSensorIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu);
-struct ParamsSensorIMU : public ParamsSensorBase
+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,12 +25,12 @@ struct ParamsSensorIMU : public ParamsSensorBase
double ab_rate_stdev = 0.00001;
double wb_rate_stdev = 0.00001;
- virtual ~ParamsSensorIMU() = default;
- ParamsSensorIMU()
+ virtual ~ParamsSensorImu() = default;
+ ParamsSensorImu()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
- ParamsSensorIMU(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorImu(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise");
@@ -52,9 +52,9 @@ struct ParamsSensorIMU : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorIMU);
+WOLF_PTR_TYPEDEFS(SensorImu);
-class SensorIMU : public SensorBase
+class SensorImu : public SensorBase
{
protected:
@@ -69,9 +69,9 @@ class SensorIMU : public SensorBase
public:
- SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params);
- SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params);
- WOLF_SENSOR_CREATE(SensorIMU, ParamsSensorIMU, 7);
+ SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params);
+ SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params);
+ WOLF_SENSOR_CREATE(SensorImu, ParamsSensorImu, 7);
double getGyroNoise() const;
double getAccelNoise() const;
@@ -80,36 +80,36 @@ class SensorIMU : public SensorBase
double getWbRateStdev() const;
double getAbRateStdev() const;
- virtual ~SensorIMU();
+ virtual ~SensorImu();
};
-inline double SensorIMU::getGyroNoise() const
+inline double SensorImu::getGyroNoise() const
{
return w_noise;
}
-inline double SensorIMU::getAccelNoise() const
+inline double SensorImu::getAccelNoise() const
{
return a_noise;
}
-inline double SensorIMU::getWbInitialStdev() const
+inline double SensorImu::getWbInitialStdev() const
{
return wb_initial_stdev;
}
-inline double SensorIMU::getAbInitialStdev() const
+inline double SensorImu::getAbInitialStdev() const
{
return ab_initial_stdev;
}
-inline double SensorIMU::getWbRateStdev() const
+inline double SensorImu::getWbRateStdev() const
{
return wb_rate_stdev;
}
-inline double SensorIMU::getAbRateStdev() const
+inline double SensorImu::getAbRateStdev() const
{
return ab_rate_stdev;
}
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 48e737fd79e53aed2e703795f30fac141dab2a7c..5e86b56d933dbe001fb8e265fac07f9a876191d9 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -5,46 +5,46 @@
namespace wolf {
-ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
- // ProcessorMotion("ProcessorImu", 3, 10, 10, 9, 6, 6, _params_motion_IMU),
- ProcessorMotion("ProcessorImu", 10, 10, 9, 6, 6, _params_motion_IMU),
- params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU))
+ProcessorImu::ProcessorImu(ProcessorParamsImuPtr _params_motion_Imu) :
+ // ProcessorMotion("ProcessorImu", 3, 10, 10, 9, 6, 6, _params_motion_Imu),
+ ProcessorMotion("ProcessorImu", 10, 10, 9, 6, 6, _params_motion_Imu),
+ params_motion_Imu_(std::make_shared<ProcessorParamsImu>(*_params_motion_Imu))
{
// Set constant parts of Jacobians
jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
jacobian_delta_.setIdentity(9,9); //
jacobian_calib_.setZero(9,6);
- unmeasured_perturbation_cov_ = pow(params_motion_IMU_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity();
+ unmeasured_perturbation_cov_ = pow(params_motion_Imu_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity();
}
-ProcessorIMU::~ProcessorIMU()
+ProcessorImu::~ProcessorImu()
{
//
}
-void ProcessorIMU::configure(SensorBasePtr _sensor)
+void ProcessorImu::configure(SensorBasePtr _sensor)
{
- auto sensor_ = std::dynamic_pointer_cast<SensorIMU>(_sensor);
- assert(sensor_ != nullptr && "Sensor is not of type SensorIMU");
+ auto sensor_ = std::dynamic_pointer_cast<SensorImu>(_sensor);
+ assert(sensor_ != nullptr && "Sensor is not of type SensorImu");
}
-bool ProcessorIMU::voteForKeyFrame() const
+bool ProcessorImu::voteForKeyFrame() const
{
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
+ if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_Imu_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > params_motion_IMU_->max_buff_length)
+ if (getBuffer().get().size() > params_motion_Imu_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer length" );
return true;
}
// angle turned
double angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
- if (angle > params_motion_IMU_->angle_turned)
+ if (angle > params_motion_Imu_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
@@ -54,7 +54,7 @@ bool ProcessorIMU::voteForKeyFrame() const
}
-CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
@@ -63,7 +63,7 @@ CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureIMU>(_frame_own,
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own,
_ts,
_sensor,
_data,
@@ -75,9 +75,9 @@ CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- auto feature = FeatureBase::emplace<FeatureIMU>(_capture_motion,
+ auto feature = FeatureBase::emplace<FeatureImu>(_capture_motion,
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
_capture_motion->getCalibrationPreint(),
@@ -85,17 +85,17 @@ FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
return feature;
}
-FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
- FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
+ CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
+ FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(_feature_motion);
- auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
return fac_imu;
}
-void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXd& _data,
+void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
@@ -137,7 +137,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXd& _data,
}
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta) const
@@ -152,7 +152,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
_delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
}
-void ProcessorIMU::statePlusDelta(const Eigen::VectorXd& _x,
+void ProcessorImu::statePlusDelta(const Eigen::VectorXd& _x,
const Eigen::VectorXd& _delta,
const double _dt,
Eigen::VectorXd& _x_plus_delta) const
@@ -165,7 +165,7 @@ void ProcessorIMU::statePlusDelta(const Eigen::VectorXd& _x,
_x_plus_delta = imu::composeOverState(_x, _delta, _dt);
}
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
const double _dt,
Eigen::VectorXd& _delta_preint_plus_delta,
@@ -205,6 +205,6 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
namespace wolf
{
-WOLF_REGISTER_PROCESSOR("ProcessorIMU", ProcessorIMU)
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorIMU", ProcessorIMU)
+WOLF_REGISTER_PROCESSOR("ProcessorImu", ProcessorImu)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorImu", ProcessorImu)
}
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index cff4178c9975f0282ae25df7522aef37bc11a3a1..927a1aea07db217557ccba5739f0df4ec020b581 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/src/sensor/sensor_imu.cpp
@@ -4,14 +4,14 @@
namespace wolf {
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params) :
- SensorIMU(_extrinsics, *_params)
+SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params) :
+ SensorImu(_extrinsics, *_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),
+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),
ab_initial_stdev(_params.ab_initial_stdev),
@@ -19,10 +19,10 @@ SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU&
ab_rate_stdev(_params.ab_rate_stdev),
wb_rate_stdev(_params.wb_rate_stdev)
{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+ assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2d.");
}
-SensorIMU::~SensorIMU()
+SensorImu::~SensorImu()
{
//
}
@@ -32,6 +32,6 @@ SensorIMU::~SensorIMU()
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorIMU", SensorIMU)
-WOLF_REGISTER_SENSOR_AUTO("SensorIMU", SensorIMU);
+WOLF_REGISTER_SENSOR("SensorImu", SensorImu)
+WOLF_REGISTER_SENSOR_AUTO("SensorImu", SensorImu);
} // namespace wolf
diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
index d4756a4dd0b6e7b0b7e705e993d52334a432249c..7e7082899ff4924749c4b402f163ac96afc2b688 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -47,7 +47,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ProcessorIMU", createProcessorIMUParams);
+const bool WOLF_UNUSED registered_prc_odom_3d = ProcessorParamsFactory::get().registerCreator("ProcessorIMU", createProcessorIMUParams);
} // namespace [unnamed]
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index b734f7665eb3b219d296e64c915c9847ca00891f..b1d7254b8ef39178d63f9e0217011dcf408a9fb2 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -15,8 +15,8 @@
#include "imu/sensor/sensor_imu.h"
//Wolf
-#include <core/factor/factor_pose_3D.h>
-#include <core/processor/processor_odom_3D.h>
+#include <core/factor/factor_pose_3d.h>
+#include <core/processor/processor_odom_3d.h>
#include <core/ceres_wrapper/ceres_manager.h>
// debug
@@ -849,14 +849,14 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
CeresManagerPtr ceres_manager;
SensorBasePtr sensor;
SensorIMUPtr sensor_imu;
- SensorOdom3DPtr sensor_odo;
+ SensorOdom3dPtr sensor_odo;
ProcessorBasePtr processor;
ProcessorIMUPtr processor_imu;
- ProcessorOdom3DPtr processor_odo;
+ ProcessorOdom3dPtr processor_odo;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
CaptureIMUPtr capture_imu;
- CaptureOdom3DPtr capture_odo;
+ CaptureOdom3dPtr capture_odo;
Eigen::Vector6d origin_bias;
Eigen::VectorXd expected_final_state;
Eigen::VectorXd x_origin;
@@ -884,23 +884,23 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
processor = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
- // SENSOR + PROCESSOR ODOM 3D
- sensor = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
- sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor);
+ // SENSOR + PROCESSOR ODOM 3d
+ sensor = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+ sensor_odo = std::static_pointer_cast<SensorOdom3d>(sensor);
sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.0099;
- prc_odom3D_params->max_buff_length = 1000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000;
- prc_odom3D_params->angle_turned = 1000;
+ ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+ prc_odom3d_params->max_time_span = 0.0099;
+ prc_odom3d_params->max_buff_length = 1000; //make it very high so that this condition will not pass
+ prc_odom3d_params->dist_traveled = 1000;
+ prc_odom3d_params->angle_turned = 1000;
- processor = problem->installProcessor("ODOM 3D", "odom", sensor_odo, prc_odom3D_params);
- processor_odo = std::static_pointer_cast<ProcessorOdom3D>(processor);
+ processor = problem->installProcessor("ODOM 3d", "odom", sensor_odo, prc_odom3d_params);
+ processor_odo = std::static_pointer_cast<ProcessorOdom3d>(processor);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -929,7 +929,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
- capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
+ capture_odo = std::make_shared<CaptureOdom3d>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
sensor_odo->process(capture_odo);
t_odo += dt_odo; //first odometry data will be processed at this timestamp
@@ -973,7 +973,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
- // PROCESS ODOM 3D DATA
+ // PROCESS ODOM 3d DATA
data_odo.head(3) << 0,0,0;
data_odo.tail(3) = q2v(quat_odo);
capture_odo->setTimeStamp(t_odo);
@@ -1030,12 +1030,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
public:
wolf::TimeStamp t;
SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
+ SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
ProcessorIMUPtr processor_imu;
- ProcessorOdom3DPtr processor_odom3D;
+ ProcessorOdom3dPtr processor_odom3d;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -1067,17 +1067,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
+ // SENSOR + PROCESSOR ODOM 3d
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+ ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+ prc_odom3d_params->max_time_span = 0.9999;
+ prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
+ prc_odom3d_params->dist_traveled = 1000000000;
+ prc_odom3d_params->angle_turned = 1000000000;
+
+ ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
+ sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen1_ptr);
+ processor_odom3d = std::static_pointer_cast<ProcessorOdom3d>(processor_odom);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -1093,13 +1093,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
//set origin of the problem
origin_KF = processor_imu->setOrigin(x_origin, t);
- processor_odom3D->setOrigin(origin_KF);
+ processor_odom3d->setOrigin(origin_KF);
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3d(Eigen::Vector6d::Zero());
Eigen::Vector3d rateOfTurn; //deg/s
rateOfTurn << 0,90,0;
VectorXd D_cor(10);
@@ -1107,8 +1107,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
double dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(0.0);
wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
- wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), nullptr);
- sen_odom3D->process(mot_ptr);
+ wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, sen_odom3d->getNoiseCov(), nullptr);
+ sen_odom3d->process(mot_ptr);
//first odometry data will be processed at this timestamp
t_odom.set(t_odom.get() + dt_odom);
@@ -1138,12 +1138,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
{
WOLF_TRACE("X_preint(t) : ", problem->getState(ts).transpose());
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
+ // PROCESS ODOM 3d DATA
+ data_odom3d.head(3) << 0,0,0;
+ data_odom3d.tail(3) = q2v(odom_quat);
mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
+ mot_ptr->setData(data_odom3d);
+ sen_odom3d->process(mot_ptr);
//prepare next odometry measurement
odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -1171,12 +1171,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
public:
wolf::TimeStamp t;
SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
+ SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
CeresManagerPtr ceres_manager_wolf_diff;
ProcessorBasePtr processor;
ProcessorIMUPtr processor_imu;
- ProcessorOdom3DPtr processor_odom3D;
+ ProcessorOdom3dPtr processor_odom3d;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -1209,17 +1209,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
+ // SENSOR + PROCESSOR ODOM 3d
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+ ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+ prc_odom3d_params->max_time_span = 0.9999;
+ prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
+ prc_odom3d_params->dist_traveled = 1000000000;
+ prc_odom3d_params->angle_turned = 1000000000;
+
+ ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
+ sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen1_ptr);
+ processor_odom3d = std::static_pointer_cast<ProcessorOdom3d>(processor_odom);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -1236,21 +1236,21 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
//set origin of the problem
origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005);
// origin_KF = processor_imu->setOrigin(x_origin, t);
-// processor_odom3D->setOrigin(origin_KF);
+// processor_odom3d->setOrigin(origin_KF);
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3d(Eigen::Vector6d::Zero());
Eigen::Vector3d rateOfTurn; //deg/s
rateOfTurn << 45,90,0;
double dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(1.0);
wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
- wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, nullptr);
- sen_odom3D->process(mot_ptr);
+ wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, nullptr);
+ sen_odom3d->process(mot_ptr);
//first odometry data will be processed at this timestamp
//t_odom.set(t_odom.get() + dt_odom);
@@ -1278,12 +1278,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
if(ts.get() >= t_odom.get())
{
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
+ // PROCESS ODOM 3d DATA
+ data_odom3d.head(3) << 0,0,0;
+ data_odom3d.tail(3) = q2v(odom_quat);
mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
+ mot_ptr->setData(data_odom3d);
+ sen_odom3d->process(mot_ptr);
//prepare next odometry measurement
odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -1317,12 +1317,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
if(ts.get() >= t_odom.get())
{
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
+ // PROCESS ODOM 3d DATA
+ data_odom3d.head(3) << 0,0,0;
+ data_odom3d.tail(3) = q2v(odom_quat);
mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
+ mot_ptr->setData(data_odom3d);
+ sen_odom3d->process(mot_ptr);
//prepare next odometry measurement
odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -2467,9 +2467,9 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
Eigen::MatrixXd featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov(5,5) = 0.1;
- auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
- auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
- FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+ auto capfix = CaptureBase::emplace<CaptureOdom3d>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+ auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+ FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(FactorBase::emplace<FactorPose3d>(ffix, ffix));
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2525,12 +2525,12 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
Eigen::MatrixXd featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov(5,5) = 0.1;
- // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr));
- auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
- // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
- // FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
- FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+ // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3d>(0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr));
+ auto capfix = CaptureBase::emplace<CaptureOdom3d>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+ // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
+ auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+ // FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(ffix->addFactor(std::make_shared<FactorPose3d>(ffix)));
+ FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(FactorBase::emplace<FactorPose3d>(ffix, ffix));
//prepare problem for solving
origin_KF->getP()->fix();
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 48fb83e9fe737427b43e6b128eae35d5824948ca..16b7d30838c92aa5ac3a490f673c924a54478e44 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -12,8 +12,8 @@
#include "imu/internal/config.h"
#include <core/common/wolf.h>
-#include <core/sensor/sensor_odom_3D.h>
-#include <core/processor/processor_odom_3D.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include <core/processor/processor_odom_3d.h>
#include <core/ceres_wrapper/ceres_manager.h>
#include <core/utils/utils_gtest.h>
@@ -590,9 +590,9 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
{
public:
// Wolf objects
- SensorOdom3DPtr sensor_odo;
- ProcessorOdom3DPtr processor_odo;
- CaptureOdom3DPtr capture_odo;
+ SensorOdom3dPtr sensor_odo;
+ ProcessorOdom3dPtr processor_odo;
+ CaptureOdom3dPtr capture_odo;
virtual void SetUp( ) override
{
@@ -603,12 +603,12 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
// ===================================== ODO
string wolf_root = _WOLF_IMU_ROOT_DIR;
- SensorBasePtr sensor = problem->installSensor ("SensorOdom3D", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" );
- ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom3D", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3D.yaml");
+ SensorBasePtr sensor = problem->installSensor ("SensorOdom3d", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml" );
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom3d", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3d.yaml");
- sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
+ sensor_odo = static_pointer_cast<SensorOdom3d>(sensor);
- processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
+ processor_odo = static_pointer_cast<ProcessorOdom3d>(processor);
processor_odo->setTimeTolerance(dt/2);
processor_odo->setVotingActive(false);
}
@@ -626,7 +626,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
Vector3d dth = log_q( q0.conjugate() * q1 );
data << dp, dth;
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
+ CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());
sensor_odo->process(capture_odo);
}
@@ -644,7 +644,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
Vector3d dth = log_q( q0.conjugate() * q1 );
data << dp, dth;
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
+ CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());
sensor_odo->process(capture_odo);
}
@@ -657,7 +657,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
// ===================================== ODO
// Process ODO for the callback to take effect
data = Vector6d::Zero();
- capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
+ capture_odo = make_shared<CaptureOdom3d>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
sensor_odo->process(capture_odo);
}