NormalDeltaOrientation3DEulerCostFunctor and NormalDeltaPose3DEulerCo…

…stFunctor: add runtime check for size of the residual weighting matrix A and set the measured change b to a constant size (was effectively already anyway expected and the case); only NormalDeltaOrientation3DEulerCostFunctor: split definition and implementation in separate parts; clarify comments; fix typos in comments; fuse_models::common::processAbsolutePose3DWithCovariance: remove unnecessary code; AbsolutePose3DStampedEulerConstraint and RelativePose3DStampedEulerConstraint: set variables to const which do not change

fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h

@@ -42,6 +42,7 @@
 #include <ceres/rotation.h>
 #include <Eigen/Core>
 
+#include <stdexcept>
 #include <vector>
 
 
@@ -89,68 +90,89 @@ class NormalDeltaOrientation3DEulerCostFunctor
    */
   NormalDeltaOrientation3DEulerCostFunctor(
     const fuse_core::MatrixXd& A,
-    const fuse_core::VectorXd& b,
-    const std::vector<Euler> &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}) :  //NOLINT
-      A_(A),
-      b_(b),
-      axes_(axes)
-  {
-  }
+    const fuse_core::Vector3d& b,
+    const std::vector<Euler> &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW});
 
   /**
    * @brief Evaluate the cost function. Used by the Ceres optimization engine.
    */
   template <typename T>
-  bool operator()(const T* const orientation1, const T* const orientation2, T* residuals) const
+  bool operator()(const T* const orientation1, const T* const orientation2, T* residuals) const;
+
+private:
+  fuse_core::MatrixXd A_;  //!< The residual weighting matrix, most likely the square root information matrix
+  fuse_core::VectorXd b_;  //!< The measured difference between orientation1 and orientation2. Its order must match
+                           //!< the values in \p axes.
+  std::vector<Euler> axes_;  //!< The Euler angle axes that we're measuring
+};
+
+NormalDeltaOrientation3DEulerCostFunctor::NormalDeltaOrientation3DEulerCostFunctor(
+  const fuse_core::MatrixXd& A,
+  const fuse_core::Vector3d& b,
+  const std::vector<Euler> &axes) :
+    A_(A),
+    b_(b),
+    axes_(axes)
+{
+  if (A_.cols() != b_.size())
+  {
+    throw std::invalid_argument("The number of columns in the residual weighting matrix A need to match the size of "
+                                "the measured difference b.");
+  }
+  if (A_.rows() != static_cast<Eigen::Index>(axes_.size()))
   {
-    using fuse_variables::Orientation3DStamped;
+    throw std::invalid_argument("The number of rows in the residual weighting matrix A need to match the size of "
+                                "the desired Euler angle axes.");
+  }
+}
+
+template <typename T>
+bool NormalDeltaOrientation3DEulerCostFunctor::operator()(
+  const T* const orientation1,
+  const T* const orientation2,
+  T* residuals) const
+{
+  using fuse_variables::Orientation3DStamped;
 
-    for (size_t i = 0; i < axes_.size(); ++i)
+  for (size_t i = 0; i < axes_.size(); ++i)
+  {
+    T angle1, angle2;
+    switch (axes_[i])
     {
-      T angle1, angle2;
-      switch (axes_[i])
+      case Euler::ROLL:
+      {
+        angle1 = fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
+        angle2 = fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
+        break;
+      }
+      case Euler::PITCH:
+      {
+        angle1 = fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
+        angle2 = fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
+        break;
+      }
+      case Euler::YAW:
+      {
+        angle1 = fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
+        angle2 = fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
+        break;
+      }
+      default:
       {
-        case Euler::ROLL:
-        {
-          angle1 = fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
-          angle2 = fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
-          break;
-        }
-        case Euler::PITCH:
-        {
-          angle1 = fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
-          angle2 = fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
-          break;
-        }
-        case Euler::YAW:
-        {
-          angle1 = fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]);
-          angle2 = fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]);
-          break;
-        }
-        default:
-        {
-          throw std::runtime_error("The provided axis specified is unknown. "
-                                   "I should probably be more informative here");
-        }
+        throw std::runtime_error("The provided axis specified is unknown. "
+                                  "I should probably be more informative here");
       }
-      const auto difference = fuse_core::wrapAngle2D(angle2 - angle1);
-      residuals[i] = fuse_core::wrapAngle2D(difference - T(b_[i]));
     }
-
-    // Scale the residuals by the square root information matrix to account for the measurement uncertainty.
-    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals_map(residuals, A_.rows());
-    residuals_map.applyOnTheLeft(A_.template cast<T>());
-
-    return true;
+    const auto difference = fuse_core::wrapAngle2D(angle2 - angle1);
+    residuals[i] = fuse_core::wrapAngle2D(difference - T(b_[i]));
   }
 
-private:
-  fuse_core::MatrixXd A_;  //!< The residual weighting matrix, most likely the square root information matrix
-  fuse_core::VectorXd b_;  //!< The measured difference between orientation1 and orientation2. Its order must match
-                           //!< the values in \p axes.
-  std::vector<Euler> axes_;  //!< The Euler angle axes that we're measuring
-};
+  // Scale the residuals by the square root information matrix to account for the measurement uncertainty.
+  Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals_map(residuals, A_.rows());
+  residuals_map.applyOnTheLeft(A_.template cast<T>());
+
+  return true;
+}
 
 }  // namespace fuse_constraints
 

fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.h

@@ -40,6 +40,7 @@
 #include <fuse_core/util.h>
 
 #include <ceres/rotation.h>
+#include <stdexcept>
 
 
 namespace fuse_constraints
@@ -61,8 +62,8 @@ namespace fuse_constraints
  *
  * Here, the matrix A can be of variable size, thereby permitting the computation of errors for partial measurements.
  * The vector b is a fixed-size 6x1, p1 and p2 are the position variables, and q1 and q2 are the quaternion orientation
- * variables. Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local
- * parameterization tangent space. In case the user is interested in implementing a cost function of the form
+ * variables. Note that the covariance submatrix for the orientation should represent errors in roll, pitch, and yaw.
+ * In case the user is interested in implementing a cost function of the form
  *
  *   cost(X) = (X - mu)^T S^{-1} (X - mu)
  *
@@ -80,7 +81,11 @@ class NormalDeltaPose3DEulerCostFunctor
    * The residual weighting matrix can vary in size, as this cost functor can be used to compute costs for partial
    * vectors. The number of rows of A will be the number of dimensions for which you want to compute the error, and the
    * number of columns in A will be fixed at 6. For example, if we just want to use the values of x and yaw, then \p A
-   * will be of size 2x6.
+   * will be of size 2x6 where the first row represents the weighting for x to all dimensions including x itself and
+   * the second row represents the weighting for yaw to all dimensions including yaw itself. For weighting with 1 and
+   * no relation to other dimensions the matrix should be:
+   * [1, 0, 0, 0, 0, 0]
+   * [0, 0, 0, 0, 0, 1]
    *
    * @param[in] A The residual weighting matrix, most likely the square root information matrix in order
    *              (dx, dy, dz, droll, dpitch, dyaw)
@@ -112,7 +117,15 @@ NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor(
     A_(A),
     b_(b),
     orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<3>())  // Orientation residuals will not be scaled
+                                                                         // within the orientation functor but here at
+                                                                         // the cost function after computation of the
+                                                                         // orientation residuals without scaling.
 {
+  if (A.cols() != b.size())
+  {
+    throw std::invalid_argument("The number of columns in the residual weighting matrix A need to match the size of "
+                                "the measured difference b.");
+  }
 }
 
 template <typename T>

fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.h

@@ -53,13 +53,13 @@ namespace fuse_constraints
  *
  * The cost function is of the form:
  *
- *   cost(x) = || A * [  p - b(0:2) ] ||^2
- *             ||     [  q - b(3:6) ] ||
+ *   cost(x) = || A * [  p                      - b(0:2) ] ||^2
+ *             ||     [  toEulerRollPitchYaw(q) - b(3:6) ] ||
  *
  * Here, the matrix A can be of variable size, thereby permitting the computation of errors for partial measurements.
  * The vector b is a fixed-size 6x1, p is the position variable, and q is the orientation variable.
- * Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local
- * parameterization tangent space. In case the user is interested in implementing a cost function of the form
+ * Note that the covariance submatrix for the orientation should represent errors in roll, pitch, and yaw.
+ * In case the user is interested in implementing a cost function of the form
  *
  *   cost(X) = (X - mu)^T S^{-1} (X - mu)
  *
@@ -77,7 +77,11 @@ class NormalPriorPose3DEulerCostFunctor
    * The residual weighting matrix can vary in size, as this cost functor can be used to compute costs for partial
    * vectors. The number of rows of A will be the number of dimensions for which you want to compute the error, and the
    * number of columns in A will be fixed at 6. For example, if we just want to use the values of x and yaw, then \p A
-   * will be of size 2x6.
+   * will be of size 2x6 where the first row represents the weighting for x to all dimensions including x itself and
+   * the second row represents the weighting for yaw to all dimensions including yaw itself. For weighting with 1 and
+   * no relation to other dimensions the matrix should be:
+   * [1, 0, 0, 0, 0, 0]
+   * [0, 0, 0, 0, 0, 1]
    *
    * @param[in] A The residual weighting matrix, most likely the square root information matrix in order
    *              (x, y, z, roll, pitch, yaw)

fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp

@@ -57,15 +57,15 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint(
   const std::vector<Euler>& angular_indices) :
     fuse_core::Constraint(source, {position.uuid(), orientation.uuid()})  // NOLINT(whitespace/braces)
 {
-  size_t total_variable_size = 6;  // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw)
-  size_t total_indices = linear_indices.size() + angular_indices.size();
+  constexpr size_t total_variable_size = 6;  // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw)
+  const size_t total_indices = linear_indices.size() + angular_indices.size();
 
   assert(partial_mean.rows() == static_cast<int>(total_indices));
   assert(partial_covariance.rows() == static_cast<int>(total_indices));
   assert(partial_covariance.cols() == static_cast<int>(total_indices));
 
   // Compute the sqrt information of the provided cov matrix
-  fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU();
+  const fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU();
 
   // Assemble a mean vector and sqrt information matrix from the provided values, but in proper Variable order
   // What are we doing here?

fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp

@@ -60,15 +60,15 @@ RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint(
       source,
       {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()})  // NOLINT(whitespace/braces)
 {
-  size_t total_variable_size = 6;  // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw)
-  size_t total_indices = linear_indices.size() + angular_indices.size();
+  constexpr size_t total_variable_size = 6;  // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw)
+  const size_t total_indices = linear_indices.size() + angular_indices.size();
 
   assert(partial_delta.rows() == static_cast<int>(total_indices));
   assert(partial_covariance.rows() == static_cast<int>(total_indices));
   assert(partial_covariance.cols() == static_cast<int>(total_indices));
 
   // Compute the sqrt information of the provided cov matrix
-  fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU();
+  const fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU();
 
   // Assemble a mean vector and sqrt information matrix from the provided values, but in proper variable order
   // What are we doing here?

fuse_models/include/fuse_models/common/sensor_config.h

@@ -98,7 +98,7 @@ std::enable_if_t<is_linear_2d<T>::value, size_t> toIndex(const std::string& dime
 /**
  * @brief Method that converts from 3D linear axis dimension names to index values
  *
- * This method is enabled only for variables that contain _only_ 2D linear quantities
+ * This method is enabled only for variables that contain _only_ 3D linear quantities
  *
  * @param[in] dimension - The dimension name to convert
  * @return the index of the enumerated dimension for that type

fuse_models/include/fuse_models/common/sensor_proc.h

@@ -433,10 +433,6 @@ inline bool processAbsolutePose3DWithCovariance(
     }
   }
 
-  // Convert the pose into tf2_2d transform
-  tf2_2d::Transform absolute_pose_2d;
-  tf2::fromMsg(transformed_message.pose.pose, absolute_pose_2d);
-
   // Create the pose variable
   auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id);
   auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id);