Skip to content
Snippets Groups Projects
Commit c406ac3a authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Merge branch 'feature/promote_unmeasured_cov_to_prc_motion' into 'devel'

Promote unmeasured cov to ProcessorMotion

See merge request mobile_robotics/wolf_projects/wolf_lib/wolf!270
parents 50e3f985 185550f3
No related branches found
No related tags found
No related merge requests found
......@@ -29,7 +29,6 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
// {
// time_tolerance = _time_tolerance;
// }
Scalar unmeasured_perturbation_std = 0.0001;
};
/**
......@@ -62,7 +61,6 @@ protected:
/// @brief Intrinsic params
ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
MatrixXs unmeasured_perturbation_cov_;
virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
......
......@@ -23,10 +23,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
struct ProcessorParamsMotion : public ProcessorParamsBase
{
Scalar max_time_span = 0.5;
unsigned int max_buff_length = 10;
Scalar dist_traveled = 5;
Scalar angle_turned = 0.5;
Scalar unmeasured_perturbation_std = 1e-4;
Scalar max_time_span = 0.5;
unsigned int max_buff_length = 10;
Scalar dist_traveled = 5;
Scalar angle_turned = 0.5;
};
/** \brief class for Motion processors
......@@ -472,6 +473,7 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
Eigen::MatrixXs unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
......
......@@ -21,7 +21,6 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{
Scalar cov_det = 1.0; // 1 rad^2
Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle
};
class ProcessorOdom2D : public ProcessorMotion
......@@ -75,7 +74,6 @@ class ProcessorOdom2D : public ProcessorMotion
protected:
ProcessorParamsOdom2DPtr params_odom_2D_;
MatrixXs unmeasured_perturbation_cov_;
// Factory method
public:
......
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 2.0 # seconds
max buffer length: 20000 # motion deltas
......
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 999999.0 # seconds
max buffer length: 999999 # motion deltas
......
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 0.9999 # seconds
max buffer length: 10000 # motion deltas
......
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 5.9999 # seconds
max buffer length: 10000 # motion deltas
......
......@@ -30,6 +30,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
params->unmeasured_perturbation_std = config["unmeasured perturbation std"].as<Scalar>();
params->max_time_span = kf_vote["max time span"] .as<Scalar>();
params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
......
......@@ -329,7 +329,7 @@ TEST(Odom2D, KF_callback)
params->angle_turned = 6.28;
params->max_time_span = 100;
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
params->unmeasured_perturbation_std = 0.000001;
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
......
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