From 24d40af66a6c128b18f96d5feffcc2ba96e3bbb7 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 10 Feb 2020 12:10:09 +0100
Subject: [PATCH] Fixes to diff drive to be usable in demo
---
include/core/sensor/sensor_diff_drive.h | 34 +++++++++++++++----------
src/sensor/sensor_diff_drive.cpp | 15 +++++++++++
2 files changed, 35 insertions(+), 14 deletions(-)
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 96af45729..28d082a23 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -17,20 +17,24 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
struct IntrinsicsDiffDrive : public IntrinsicsBase
{
- double radius_left;
- double radius_right;
- double wheel_separation;
- double ticks_per_wheel_revolution;
+ double radius_left;
+ double radius_right;
+ double wheel_separation;
+ double ticks_per_wheel_revolution;
+ bool set_intrinsics_prior;
+ Eigen::Vector3d prior_cov_diag;
+
+ IntrinsicsDiffDrive() = default;
+ IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
+ : IntrinsicsBase(_unique_name, _server)
+ {
+ radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
+ radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right");
+ wheel_separation = _server.getParam<double>(prefix + _unique_name + "/wheel_separation");
+ ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution");
+ set_intrinsics_prior = _server.getParam<bool>(prefix + _unique_name + "/set_intrinsics_prior");
+ prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
-
- IntrinsicsDiffDrive() = default;
- IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
- IntrinsicsBase(_unique_name, _server)
- {
- radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
- radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right");
- wheel_separation = _server.getParam<double>(prefix + _unique_name + "/wheel_separation");
- ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution");
}
std::string print() const
{
@@ -38,7 +42,9 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
+ "radius_left: " + std::to_string(radius_left) + "\n"
+ "radius_right: " + std::to_string(radius_right) + "\n"
+ "wheel_separation: " + std::to_string(wheel_separation) + "\n"
- + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n";
+ + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
+ + "set_intrinsics_prior: " + std::to_string(set_intrinsics_prior) + "\n"
+ + "prior_cov_diag: to_string not implemented yet! \n";
}
};
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index ace8ba8f0..5b85000ec 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -22,6 +22,21 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
+ std::cout << "Prior cov diag " << params_diff_drive_->prior_cov_diag.transpose() << std::endl;
+ if(params_diff_drive_->set_intrinsics_prior)
+ addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
+
+ // compute noise covariance
+ // 1. measured wheel revolutions: sigma = 2*radians_per_tick
+ double sigma_rev = 2*radians_per_tick;
+ Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
+
+// // 2. unmeasured lateral wheel slippage: sigma = 1mm
+// double sigma_slippage = 0.001;
+// Eigen::Vector3d noise_sigma; noise_sigma << sigma_rev, sigma_rev, sigma_slippage;
+
+ setNoiseStd(noise_sigma);
+
}
SensorDiffDrive::~SensorDiffDrive()
--
GitLab