diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 676cb6cd05d51240cad7a5008ca301ccb5af67e3..438dcc33b0e02f62c241b6852cd5d4fd29c4184e 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -114,13 +114,6 @@ class ProcessorOdom3d : public ProcessorMotion
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
- // noise parameters (stolen from owner SensorOdom3d)
- double k_disp_to_disp_; // displacement variance growth per meter of linear motion
- double k_disp_to_rot_; // orientation variance growth per meter of linear motion
- double k_rot_to_rot_; // orientation variance growth per radian of rotational motion
- double min_disp_var_; // floor displacement variance when no motion
- double min_rot_var_; // floor orientation variance when no motion
-
};
inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index 5f2fc1a717b7c58f985ede8845ea2de01dc17fcb..ca06c7c010bc153756650f35c55e01114fdf4e34 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -62,6 +62,25 @@ class SensorOdom2d : public SensorBase
**/
double getRotVarToRotNoiseFactor() const;
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = k_disp_to_disp * displacement
+ * - rot_var = k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix3d computeCovFromMotion (const VectorXd& _data) const;
+
+
};
} // namespace wolf
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index ab0e8b46872c21015b02ffcb9d521dbd77e2a606..d5f5b417eb36a0628a3425822d03b3fef494fb36 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -72,6 +72,26 @@ class SensorOdom3d : public SensorBase
double getMinDispVar() const;
double getMinRotVar() const;
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a minimal value for displacement
+ * - a minimal value for rotation
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = disp_var_min + k_disp_to_disp * displacement
+ * - rot_var = rot_var_min + k_disp_to_rot * displacement + k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix6d computeCovFromMotion (const VectorXd& _data) const;
+
};
inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index c7be4d48c4d086d5d4f594dfa7849419d8cc5c30..8d8b190eef3ca6836e75b2868e40c56afea83fb4 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -6,12 +6,7 @@ namespace wolf
ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
- params_odom_3d_ (_params),
- k_disp_to_disp_ (0),
- k_disp_to_rot_ (0),
- k_rot_to_rot_ (0),
- min_disp_var_ (0.1), // around 10cm error
- min_rot_var_ (0.1) // around 6 degrees error
+ params_odom_3d_ (_params)
{
// Set constant parts of Jacobians
jacobian_delta_preint_.setIdentity(6,6);
@@ -29,13 +24,6 @@ void ProcessorOdom3d::configure(SensorBasePtr _sensor)
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
-
- // we steal the parameters from the provided odom3d sensor.
- k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
- k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
- k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
- min_disp_var_ = sen_ptr->getMinDispVar();
- min_rot_var_ = sen_ptr->getMinRotVar();
}
void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index ec7a3b54d3f8163f44276bf8120cb482a3a2fdd7..a3fe8ee9aa2a81849fb0caa2789d785cdafcf22a 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -34,6 +34,17 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
return k_rot_to_rot_;
}
+Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+{
+ double d = fabs(_data(0));
+ double r = fabs(_data(1));
+
+ double dvar = k_disp_to_disp_ * d;
+ double rvar = k_rot_to_rot_ * r;
+
+ return (Matrix3d() << dvar, dvar, rvar).finished();
+}
+
}
// Register in the FactorySensor
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index 58b9b62ee9e111ce84887ba16e728a3494e40b7a..5954f505c0daf3e37588e0b78bf2b3b440b1052f 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -9,6 +9,7 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
namespace wolf {
@@ -37,6 +38,21 @@ SensorOdom3d::~SensorOdom3d()
//
}
+Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
+{
+ double d = _data.head<3>().norm();
+ double r;
+ if (_data.size() == 6)
+ r = _data.tail<3>().norm();
+ else
+ r = 2 * acos(_data(6)); // arc cos of real part of quaternion
+
+ double dvar = min_disp_var_ + k_disp_to_disp_ * d;
+ double rvar = min_rot_var_ + k_disp_to_rot_ * d + k_rot_to_rot_ * r;
+
+ return (Matrix6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished();
+}
+
} // namespace wolf
// Register in the FactorySensor
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index dc24beddd0643d7d64d21deeb59c2068752d50ec..876a4450d65e0da3231f1f9b91b9e56e3b97ac69 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -44,18 +44,10 @@ class ProcessorOdom3dTest : public ProcessorOdom3d
{
public:
ProcessorOdom3dTest();
-
- // getters :-D !!
- double& kdd() {return k_disp_to_disp_;}
- double& kdr() {return k_disp_to_rot_;}
- double& krr() {return k_rot_to_rot_;}
- double& dvar_min() {return min_disp_var_;}
- double& rvar_min() {return min_rot_var_;}
};
ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
{
- dvar_min() = 0.5;
- rvar_min() = 0.25;
+ //
}
TEST(ProcessorOdom3d, computeCurrentDelta)
@@ -76,11 +68,8 @@ TEST(ProcessorOdom3d, computeCurrentDelta)
delta.head<3>() = delta_dp;
delta.tail<4>() = delta_dq.coeffs();
- // construct covariance from processor parameters and motion magnitudes
- double disp = data_dp.norm();
- double rot = data_do.norm();
- double dvar = prc.dvar_min() + prc.kdd()*disp;
- double rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot;
+ double dvar = 0.1;
+ double rvar = 0.2;
Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
Matrix6d data_cov = diag.asDiagonal();
Matrix6d delta_cov = data_cov;
@@ -119,8 +108,6 @@ TEST(ProcessorOdom3d, deltaPlusDelta)
prc.deltaPlusDelta(D, d, dt, D_int);
ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
-// << "\nDpd : " << D_int.transpose()
-// << "\ncheck: " << D_int_check.transpose();
}
TEST(ProcessorOdom3d, deltaPlusDelta_Jac)