Fixes to diff drive to be usable in demo

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";
         }
 
 };

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()