Rotate position in motion model constraint

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

@@ -42,13 +42,11 @@
 
 #include <ceres/sized_cost_function.h>
 
-
 namespace fuse_models
 {
-
 /**
  * @brief Create a cost function for a 2D state vector
- * 
+ *
  * The state vector includes the following quantities, given in this order:
  *   x position
  *   y position
@@ -72,7 +70,7 @@ namespace fuse_models
  *             ||    [  yaw_vel_t2 - proj(yaw_vel_t1) ] ||
  *             ||    [    x_acc_t2 - proj(x_acc_t1)   ] ||
  *             ||    [    y_acc_t2 - proj(y_acc_t1)   ] ||
- * 
+ *
  * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function
  * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost
  * function of the form
@@ -115,52 +113,52 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
    *                         computed for the parameters where jacobians[i] is not NULL.
    * @return The return value indicates whether the computation of the residuals and/or jacobians was successful or not.
    */
-  bool Evaluate(double const* const* parameters,
-                double* residuals,
-                double** jacobians) const override
+  bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
   {
-    double position_pred_x;
-    double position_pred_y;
-    double yaw_pred;
-    double vel_linear_pred_x;
-    double vel_linear_pred_y;
+    fuse_core::Vector2d delta_position_pred;
+    double delta_yaw_pred;
+    fuse_core::Vector2d vel_linear_pred;
     double vel_yaw_pred;
-    double acc_linear_pred_x;
-    double acc_linear_pred_y;
+    fuse_core::Vector2d acc_linear_pred;
     predict(
-      parameters[0][0],  // position1_x
-      parameters[0][1],  // position1_y
-      parameters[1][0],  // yaw1
+      0.0,
+      0.0,
+      0.0,
       parameters[2][0],  // vel_linear1_x
       parameters[2][1],  // vel_linear1_y
       parameters[3][0],  // vel_yaw1
       parameters[4][0],  // acc_linear1_x
       parameters[4][1],  // acc_linear1_y
       dt_,
-      position_pred_x,
-      position_pred_y,
-      yaw_pred,
-      vel_linear_pred_x,
-      vel_linear_pred_y,
+      delta_position_pred.x(),
+      delta_position_pred.y(),
+      delta_yaw_pred,
+      vel_linear_pred.x(),
+      vel_linear_pred.y(),
       vel_yaw_pred,
-      acc_linear_pred_x,
-      acc_linear_pred_y,
+      acc_linear_pred.x(),
+      acc_linear_pred.y(),
       jacobians);
 
-    residuals[0] = parameters[5][0] - position_pred_x;
-    residuals[1] = parameters[5][1] - position_pred_y;
-    residuals[2] = parameters[6][0] - yaw_pred;
-    residuals[3] = parameters[7][0] - vel_linear_pred_x;
-    residuals[4] = parameters[7][1] - vel_linear_pred_y;
-    residuals[5] = parameters[8][0] - vel_yaw_pred;
-    residuals[6] = parameters[9][0] - acc_linear_pred_x;
-    residuals[7] = parameters[9][1] - acc_linear_pred_y;
+    const double delta_yaw_est = parameters[6][0] - parameters[1][0];
+    const fuse_core::Vector2d position1(parameters[0][0], parameters[0][1]);
+    const fuse_core::Vector2d position2(parameters[5][0], parameters[5][1]);
+    const fuse_core::Vector2d position_diff = position2 - position1;
+    const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]);
 
-    fuse_core::wrapAngle2D(residuals[2]);
+    Eigen::Map<fuse_core::Vector8d> residuals_map(residuals);
+    residuals_map.head<2>() = R_yaw_inv * position_diff - delta_position_pred;
+    residuals_map(2) = delta_yaw_est - delta_yaw_pred;
+    residuals_map(3) = parameters[7][0] - vel_linear_pred.x();
+    residuals_map(4) = parameters[7][1] - vel_linear_pred.y();
+    residuals_map(5) = parameters[8][0] - vel_yaw_pred;
+    residuals_map(6) = parameters[9][0] - acc_linear_pred.x();
+    residuals_map(7) = parameters[9][1] - acc_linear_pred.y();
+
+  fuse_core::wrapAngle2D(residuals_map(2));
 
     // Scale the residuals by the square root information matrix to account for
     // the measurement uncertainty.
-    Eigen::Map<fuse_core::Vector8d> residuals_map(residuals);
     residuals_map.applyOnTheLeft(A_);
 
     if (jacobians)
@@ -182,6 +180,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
       if (jacobians[0])
       {
         Eigen::Map<fuse_core::Matrix<double, 8, 2>> jacobian(jacobians[0]);
+        jacobian.block<2, 2>(0, 0).applyOnTheLeft(R_yaw_inv);
         jacobian.applyOnTheLeft(-A_);
       }
 
@@ -235,6 +234,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
       {
         Eigen::Map<fuse_core::Matrix<double, 8, 2>> jacobian(jacobians[5]);
         jacobian = A_.block<8, 2>(0, 0);
+        jacobian.block<2, 2>(0, 0) *= R_yaw_inv;
       }
 
       // Jacobian wrt yaw2
@@ -274,9 +274,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
   fuse_core::Matrix8d A_;  //!< The residual weighting matrix, most likely the square root information matrix
 };
 
-Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) :
-  dt_(dt),
-  A_(A)
+Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A)
 {
 }
 

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

@@ -40,13 +40,11 @@
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/util.h>
 
-
 namespace fuse_models
 {
-
 /**
  * @brief Create a cost function for a 2D state vector
- * 
+ *
  * The state vector includes the following quantities, given in this order:
  *   x position
  *   y position
@@ -70,7 +68,7 @@ namespace fuse_models
  *             ||    [  yaw_vel_t2 - proj(yaw_vel_t1) ] ||
  *             ||    [    x_acc_t2 - proj(x_acc_t1)   ] ||
  *             ||    [    y_acc_t2 - proj(y_acc_t1)   ] ||
- * 
+ *
  * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function
  * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost
  * function of the form
@@ -127,9 +125,7 @@ class Unicycle2DStateCostFunctor
   fuse_core::Matrix8d A_;  //!< The residual weighting matrix, most likely the square root information matrix
 };
 
-Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) :
-  dt_(dt),
-  A_(A)
+Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A)
 {
 }
 
@@ -147,28 +143,39 @@ bool Unicycle2DStateCostFunctor::operator()(
   const T* const acc_linear2,
   T* residual) const
 {
-  T position_pred[2];
-  T yaw_pred[1];
+  T delta_position_pred[2];
+  T delta_yaw_pred[1];
   T vel_linear_pred[2];
   T vel_yaw_pred[1];
   T acc_linear_pred[2];
+  T init_position[2];
+  T init_yaw[1];
+  init_position[0] = T(0.0);
+  init_position[1] = T(0.0);
+  init_yaw[0] = T(0.0);
   predict(
-    position1,
-    yaw1,
+    init_position,
+    init_yaw,
     vel_linear1,
     vel_yaw1,
     acc_linear1,
     T(dt_),
-    position_pred,
-    yaw_pred,
+    delta_position_pred,
+    delta_yaw_pred,
     vel_linear_pred,
     vel_yaw_pred,
     acc_linear_pred);
 
+  const T delta_yaw_est = yaw2[0] - yaw1[0];
+  const Eigen::Matrix<T, 2, 1> position1_map(position1[0], position1[1]);
+  const Eigen::Matrix<T, 2, 1> position2_map(position2[0], position2[1]);
+  const Eigen::Matrix<T, 2, 1> position_diff = (position2_map - position1_map);
+  const fuse_core::Matrix<T, 2, 2> R_yaw_inv = fuse_core::rotationMatrix2D(-yaw1[0]);
+  const Eigen::Map<Eigen::Matrix<T, 2, 1>> delta_position_pred_map(delta_position_pred);
+
   Eigen::Map<Eigen::Matrix<T, 8, 1>> residuals_map(residual);
-  residuals_map(0) = position2[0] - position_pred[0];
-  residuals_map(1) = position2[1] - position_pred[1];
-  residuals_map(2) = yaw2[0] - yaw_pred[0];
+  residuals_map.template head<2>() = R_yaw_inv * position_diff - delta_position_pred_map;
+  residuals_map(2) = delta_yaw_est - delta_yaw_pred[0];
   residuals_map(3) = vel_linear2[0] - vel_linear_pred[0];
   residuals_map(4) = vel_linear2[1] - vel_linear_pred[1];
   residuals_map(5) = vel_yaw2[0] - vel_yaw_pred[0];