Skip to content
Snippets Groups Projects
Commit 6a2f1f2a authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Moved some files WIP

parent fc7388e2
No related branches found
No related tags found
2 merge requests!39release after RAL,!38After 2nd RAL submission
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
......
File moved
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
......
File moved
......@@ -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
}
......
......@@ -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;
}
......
......@@ -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)
}
......@@ -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
......@@ -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]
......
......@@ -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();
......
......@@ -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);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment