RST-1239 Adding 3D relative pose constraint (#27)

* Adding 3D relative pose constraint
* Updating expected covariance matrices for anything involving rotations

fuse_constraints/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(${PROJECT_NAME}
   src/normal_delta_orientation_2d.cpp
   src/normal_prior_orientation_2d.cpp
   src/relative_pose_2d_stamped_constraint.cpp
+  src/relative_pose_3d_stamped_constraint.cpp
 )
 add_dependencies(${PROJECT_NAME}
   ${catkin_EXPORTED_TARGETS}
@@ -171,4 +172,16 @@ if(CATKIN_ENABLE_TESTING)
     ${PROJECT_NAME}
     ${catkin_LIBRARIES}
   )
+
+  # Relative Pose 3D Stamped Constraint Tests
+  catkin_add_gtest(test_relative_pose_3d_stamped_constraint
+    test/test_relative_pose_3d_stamped_constraint.cpp
+  )
+  add_dependencies(test_relative_pose_3d_stamped_constraint
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_link_libraries(test_relative_pose_3d_stamped_constraint
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+  )
 endif()

fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h

@@ -0,0 +1,179 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H
+#define FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H
+
+#include <fuse_constraints/util.h>
+#include <fuse_core/eigen.h>
+
+#include <ceres/rotation.h>
+
+namespace fuse_constraints
+{
+
+/**
+ * @brief Implements a cost function that models a difference between 3D pose variables.
+ *
+ * A single pose involves two variables: a 3D position and a 3D orientation. This cost function computes the difference
+ * using standard 3D transformation math:
+ *
+ *   delta = q1^-1 * [position2 - position1]
+ *                   [          q2         ]
+ *
+ * where q1 and q2 are the orientations of the two poses, given as quaternions. Once the delta is computed, the
+ * difference between the computed delta and the expected delta is given as follows:
+ *
+ *             ||    [       delta(0) - b(0)       ] ||^2
+ *   cost(x) = ||    [       delta(1) - b(1)       ] ||
+ *             ||A * [       delta(2) - b(2)       ] ||
+ *             ||    [ (delta(3:6) * b(3:6)^-1)(1) ] ||
+ *             ||    [ (delta(3:6) * b(3:6)^-1)(2) ] ||
+ *             ||    [ (delta(3:6) * b(3:6)^-1)(3) ] ||
+ *
+ * where, the matrix A and the vector b are fixed. In case the user is interested in implementing a cost function of
+ * the form:
+ *
+ *   cost(X) = (X - mu)^T S^{-1} (X - mu)
+ *
+ * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the square root
+ * information matrix (the inverse of the covariance).
+ *
+ * Note that the cost function's quaternion components are only concerned with the imaginary components (qx, qy, qz).
+ */
+class NormalDeltaPose3DCostFunctor
+{
+public:
+  /**
+   * @brief Constructor
+   *
+   * @param[in] A The residual weighting matrix, most likely the square root information matrix in order
+   *              (dx, dy, dz, dqx, dqy, dqz)
+   * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz)
+   */
+  NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b);
+
+  /**
+   * @brief Compute the cost values/residuals using the provided variable/parameter values
+   */
+  template <typename T>
+  bool operator()(
+    const T* const position1,
+    const T* const orientation1,
+    const T* const position2,
+    const T* const orientation2,
+    T* residual) const;
+
+private:
+  fuse_core::Matrix6d A_;  //!< The residual weighting matrix, most likely the square root information matrix
+  fuse_core::Vector7d b_;  //!< The measured difference between variable pose1 and variable pose2
+};
+
+NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) :
+  A_(A),
+  b_(b)
+{
+}
+
+template <typename T>
+bool NormalDeltaPose3DCostFunctor::operator()(
+  const T* const position1,
+  const T* const orientation1,
+  const T* const position2,
+  const T* const orientation2,
+  T* residual) const
+{
+  // Based on the Ceres SLAM pose graph error calculation here:
+  // https://github.com/ceres-solver/ceres-solver/blob/4fc5d25f9cbfe2aa333425ddad03bdc651335c24/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h#L73
+
+  // 1. Get the relative orientation change from the variable's orientation1 to orientation2
+  T variable_orientation1_inverse[4] =
+  {
+    orientation1[0],
+    -orientation1[1],
+    -orientation1[2],
+    -orientation1[3]
+  };
+
+  T variable_orientation_delta[4];
+  ceres::QuaternionProduct(variable_orientation1_inverse, orientation2, variable_orientation_delta);
+
+  // 2. Get the position change from pose1 to pose2, then rotate it into the frame of pose1
+  T variable_position_delta[3] =
+  {
+    position2[0] - position1[0],
+    position2[1] - position1[1],
+    position2[2] - position1[2]
+  };
+
+  T variable_position_delta_rotated[3];
+  ceres::QuaternionRotatePoint(
+    variable_orientation1_inverse,
+    variable_position_delta,
+    variable_position_delta_rotated);
+
+  // 3. Get the difference between the orientation delta that we just computed for orientation1 and orientation2,
+  // and the measurement's orientation delta
+  T observation_inverse[4] =
+  {
+    T(b_(3)),
+    T(-b_(4)),
+    T(-b_(5)),
+    T(-b_(6))
+  };
+
+  T delta_difference_orientation[4];
+  ceres::QuaternionProduct(
+    variable_orientation_delta,
+    observation_inverse,
+    delta_difference_orientation);
+
+  // 4. Compute the position delta, and throw everything into a residual at the same time
+  residual[0] = variable_position_delta_rotated[0] - b_[0];
+  residual[1] = variable_position_delta_rotated[1] - b_[1];
+  residual[2] = variable_position_delta_rotated[2] - b_[2];
+  residual[3] = T(2.0) * delta_difference_orientation[1];
+  residual[4] = T(2.0) * delta_difference_orientation[2];
+  residual[5] = T(2.0) * delta_difference_orientation[3];
+
+  // 5. Map it to Eigen, and weight it
+  Eigen::Map<Eigen::Matrix<T, 6, 1> > residual_map(residual);
+
+  residual_map.applyOnTheLeft(A_.template cast<T>());
+
+  return true;
+}
+
+}  // namespace fuse_constraints
+
+#endif  // FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H

fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h

@@ -100,7 +100,7 @@ class NormalPriorOrientation3DCostFunctor
       orientation[3]
     };
 
-    T inverse_observation[4] =
+    T observation_inverse[4] =
     {
       T(b_(0)),
       T(-b_(1)),
@@ -110,12 +110,12 @@ class NormalPriorOrientation3DCostFunctor
 
     T output[4];
 
-    ceres::QuaternionProduct(inverse_observation, variable, output);
+    ceres::QuaternionProduct(variable, observation_inverse, output);
 
     // 2. Can use just the imaginary coefficients as the residual
-    residuals[0] = output[1];
-    residuals[1] = output[2];
-    residuals[2] = output[3];
+    residuals[0] = T(2.0) * output[1];
+    residuals[1] = T(2.0) * output[2];
+    residuals[2] = T(2.0) * output[3];
 
     // 3. 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());

fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h

@@ -0,0 +1,137 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H
+#define FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H
+
+#include <fuse_core/constraint.h>
+#include <fuse_core/eigen.h>
+#include <fuse_core/macros.h>
+#include <fuse_core/uuid.h>
+#include <fuse_variables/orientation_3d_stamped.h>
+#include <fuse_variables/position_3d_stamped.h>
+
+#include <Eigen/Dense>
+
+#include <ostream>
+#include <vector>
+
+
+namespace fuse_constraints
+{
+
+/**
+ * @brief A constraint that represents a measurement on the difference between two 3D poses.
+ *
+ * This type of constraint arises in many situations. Many types of incremental odometry measurements (e.g., visual
+ * odometry) measure the change in the pose, not the pose directly. This constraint holds the measured 3D pose change
+ * and the measurement uncertainty/covariance.
+ */
+class RelativePose3DStampedConstraint : public fuse_core::Constraint
+{
+public:
+  SMART_PTR_DEFINITIONS(RelativePose3DStampedConstraint);
+
+  /**
+   * @brief Constructor
+   *
+   * @param[in] position1    The variable representing the position components of the first pose
+   * @param[in] orientation1 The variable representing the orientation components of the first pose
+   * @param[in] position2    The variable representing the position components of the second pose
+   * @param[in] orientation2 The variable representing the orientation components of the second pose
+   * @param[in] delta        The measured change in the pose (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz)
+   * @param[in] covariance   The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz)
+   */
+  RelativePose3DStampedConstraint(
+    const fuse_variables::Position3DStamped& position1,
+    const fuse_variables::Orientation3DStamped& orientation1,
+    const fuse_variables::Position3DStamped& position2,
+    const fuse_variables::Orientation3DStamped& orientation2,
+    const fuse_core::Vector7d& delta,
+    const fuse_core::Matrix6d& covariance);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~RelativePose3DStampedConstraint() = default;
+
+  /**
+   * @brief Read-only access to the measured pose change.
+   */
+  const fuse_core::Vector7d& delta() const { return delta_; }
+
+  /**
+   * @brief Read-only access to the square root information matrix.
+   */
+  const fuse_core::Matrix6d& sqrtInformation() const { return sqrt_information_; }
+
+  /**
+   * @brief Compute the measurement covariance matrix.
+   */
+  fuse_core::Matrix6d covariance() const { return (sqrt_information_.transpose() * sqrt_information_).inverse(); }
+
+  /**
+   * @brief Print a human-readable description of the constraint to the provided stream.
+   *
+   * @param[out] stream The stream to write to. Defaults to stdout.
+   */
+  void print(std::ostream& stream = std::cout) const override;
+
+  /**
+   * @brief Perform a deep copy of the constraint and return a unique pointer to the copy
+   *
+   * Unique pointers can be implicitly upgraded to shared pointers if needed.
+   *
+   * @return A unique pointer to a new instance of the most-derived constraint
+   */
+  fuse_core::Constraint::UniquePtr clone() const override;
+
+  /**
+   * @brief Access the cost function for this constraint
+   *
+   * The function caller will own the new cost function instance. It is the responsibility of the caller to delete
+   * the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the
+   * Ceres::Problem object will takes ownership of the pointer and delete it during destruction.
+   *
+   * @return A base pointer to an instance of a derived CostFunction.
+   */
+  ceres::CostFunction* costFunction() const override;
+
+protected:
+  fuse_core::Vector7d delta_;  //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz)
+  fuse_core::Matrix6d sqrt_information_;  //!< The square root information matrix (derived from the covariance matrix)
+};
+
+}  // namespace fuse_constraints
+
+#endif  // FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H