From 185550f365f06a2afb9d2d204210ac764a463332 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Mon, 29 Apr 2019 16:44:03 +0200
Subject: [PATCH] Promote unmeasured cov to ProcessorMotion
---
include/base/processor/processor_diff_drive.h | 2 --
include/base/processor/processor_motion.h | 10 ++++++----
include/base/processor/processor_odom_2D.h | 2 --
src/examples/processor_imu.yaml | 1 +
src/examples/processor_imu_no_vote.yaml | 1 +
src/examples/processor_imu_t1.yaml | 1 +
src/examples/processor_imu_t6.yaml | 1 +
src/yaml/processor_IMU_yaml.cpp | 1 +
test/gtest_odom_2D.cpp | 2 +-
9 files changed, 12 insertions(+), 9 deletions(-)
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index 4ea5181b8..9d05bab54 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -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,
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 46af97f52..38a4faa0d 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -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
};
}
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index ae73d0e93..5def5e3ad 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -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:
diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
index 7e684c883..9d3d4fb80 100644
--- a/src/examples/processor_imu.yaml
+++ b/src/examples/processor_imu.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
index 4f6ad3955..4a1dd140e 100644
--- a/src/examples/processor_imu_no_vote.yaml
+++ b/src/examples/processor_imu_no_vote.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
index e0c21758c..e290b5ac4 100644
--- a/src/examples/processor_imu_t1.yaml
+++ b/src/examples/processor_imu_t1.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
index e3a4b17df..b864e9859 100644
--- a/src/examples/processor_imu_t6.yaml
+++ b/src/examples/processor_imu_t6.yaml
@@ -1,5 +1,6 @@
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
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 5b3f12ac6..414648494 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -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>();
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 2aa224b31..604ef4894 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -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);
--
GitLab