Add extrinsic support to all constraints

Author: jakemclaughlin6

vesta_constraints/include/vesta_constraints/3d/absolute_orientation_3d_stamped_constraint.h

@@ -39,6 +39,8 @@
 #include <vesta_core/fuse_macros.h>
 #include <vesta_core/serialization.h>
 #include <vesta_core/uuid.h>
+#include <vesta_variables/3d/extrinsic_3d_orientation.h>
+#include <vesta_variables/3d/extrinsic_3d_position.h>
 #include <vesta_variables/3d/orientation_3d_stamped.h>
 
 #include <Eigen/Geometry>
@@ -102,6 +104,34 @@ class AbsoluteOrientation3DStampedConstraint : public vesta_core::Constraint
                                          const vesta_variables::Orientation3DStamped& orientation,
                                          const Eigen::Quaterniond& mean, const vesta_core::Matrix3d& covariance);
 
+  /**
+   * @brief Constructor with extrinsic calibration
+   *
+   * This constructor accepts an extrinsic transform. The orientation variable represents the body
+   * frame, and the extrinsic rotation is applied internally to compute the sensor-frame orientation.
+   * The extrinsic translation is included for API consistency but does not affect orientation.
+   *
+   * @param[in] source          The name of the sensor or motion model that generated this constraint
+   * @param[in] orientation     The variable representing the body-frame orientation
+   * @param[in] ext_position    The extrinsic translation (body-to-sensor), unused in math
+   * @param[in] ext_orientation The extrinsic rotation (body-to-sensor)
+   * @param[in] mean            The measured/prior sensor-frame orientation (4x1 vector: w, x, y, z)
+   * @param[in] covariance      The measurement/prior covariance (3x3 matrix: qx, qy, qz)
+   */
+  AbsoluteOrientation3DStampedConstraint(const std::string& source,
+                                         const vesta_variables::Orientation3DStamped& orientation,
+                                         const vesta_variables::Extrinsic3DPosition& ext_position,
+                                         const vesta_variables::Extrinsic3DOrientation& ext_orientation,
+                                         const vesta_core::Vector4d& mean, const vesta_core::Matrix3d& covariance);
+
+  /**
+   * @brief Returns whether this constraint uses an extrinsic calibration.
+   */
+  bool hasExtrinsic() const
+  {
+    return has_extrinsic_;
+  }
+
   /**
    * @brief Destructor
    */
@@ -172,6 +202,7 @@ class AbsoluteOrientation3DStampedConstraint : public vesta_core::Constraint
 
   vesta_core::Vector4d mean_;              //!< The measured/prior mean vector for this variable
   vesta_core::Matrix3d sqrt_information_;  //!< The square root information matrix
+  bool has_extrinsic_{ false };            //!< Whether this constraint uses an extrinsic calibration
 
 private:
   // Allow Boost Serialization access to private methods
@@ -192,6 +223,7 @@ class AbsoluteOrientation3DStampedConstraint : public vesta_core::Constraint
     archive& boost::serialization::base_object<vesta_core::Constraint>(*this);
     archive & mean_;
     archive & sqrt_information_;
+    archive & has_extrinsic_;
   }
 };
 

vesta_constraints/include/vesta_constraints/3d/absolute_orientation_3d_stamped_euler_constraint.h

@@ -39,6 +39,8 @@
 #include <vesta_core/fuse_macros.h>
 #include <vesta_core/serialization.h>
 #include <vesta_core/uuid.h>
+#include <vesta_variables/3d/extrinsic_3d_orientation.h>
+#include <vesta_variables/3d/extrinsic_3d_position.h>
 #include <vesta_variables/3d/orientation_3d_stamped.h>
 
 #include <boost/serialization/access.hpp>
@@ -94,6 +96,37 @@ class AbsoluteOrientation3DStampedEulerConstraint : public vesta_core::Constrain
                                               const vesta_core::VectorXd& mean, const vesta_core::MatrixXd& covariance,
                                               const std::vector<Euler>& axes);
 
+  /**
+   * @brief Constructor with extrinsic calibration
+   *
+   * This constructor accepts an extrinsic transform. The orientation variable represents the body
+   * frame, and the extrinsic rotation is applied internally to compute the sensor-frame orientation
+   * before extracting Euler angles. The extrinsic translation is included for API consistency but
+   * does not affect orientation.
+   *
+   * @param[in] source          The name of the sensor or motion model that generated this constraint
+   * @param[in] orientation     The variable representing the body-frame orientation
+   * @param[in] ext_position    The extrinsic translation (body-to-sensor), unused in math
+   * @param[in] ext_orientation The extrinsic rotation (body-to-sensor)
+   * @param[in] mean            The measured/prior Euler orientations in the order specified in \p axes
+   * @param[in] covariance      The measurement/prior covariance
+   * @param[in] axes            Used to specify which Euler axes to include in the constraint
+   */
+  AbsoluteOrientation3DStampedEulerConstraint(const std::string& source,
+                                              const vesta_variables::Orientation3DStamped& orientation,
+                                              const vesta_variables::Extrinsic3DPosition& ext_position,
+                                              const vesta_variables::Extrinsic3DOrientation& ext_orientation,
+                                              const vesta_core::VectorXd& mean, const vesta_core::MatrixXd& covariance,
+                                              const std::vector<Euler>& axes);
+
+  /**
+   * @brief Returns whether this constraint uses an extrinsic calibration.
+   */
+  bool hasExtrinsic() const
+  {
+    return has_extrinsic_;
+  }
+
   /**
    * @brief Destructor
    */
@@ -166,6 +199,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public vesta_core::Constrain
   vesta_core::VectorXd mean_;              //!< The measured/prior mean vector for this variable
   vesta_core::MatrixXd sqrt_information_;  //!< The square root information matrix
   std::vector<Euler> axes_;                //!< Which Euler angle axes we want to measure
+  bool has_extrinsic_{ false };            //!< Whether this constraint uses an extrinsic calibration
 
 private:
   // Allow Boost Serialization access to private methods
@@ -187,6 +221,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public vesta_core::Constrain
     archive & mean_;
     archive & sqrt_information_;
     archive & axes_;
+    archive & has_extrinsic_;
   }
 };
 

vesta_constraints/include/vesta_constraints/3d/absolute_pose_3d_stamped_constraint.h

@@ -39,6 +39,8 @@
 #include <vesta_core/fuse_macros.h>
 #include <vesta_core/serialization.h>
 #include <vesta_core/uuid.h>
+#include <vesta_variables/3d/extrinsic_3d_orientation.h>
+#include <vesta_variables/3d/extrinsic_3d_position.h>
 #include <vesta_variables/3d/orientation_3d_stamped.h>
 #include <vesta_variables/3d/position_3d_stamped.h>
 
@@ -96,6 +98,35 @@ class AbsolutePose3DStampedConstraint : public vesta_core::Constraint
                                   const vesta_variables::Orientation3DStamped& orientation,
                                   const vesta_core::Vector7d& mean, const vesta_core::Matrix6d& covariance);
 
+  /**
+   * @brief Constructor with extrinsic calibration
+   *
+   * This constructor accepts an extrinsic transform T_body_sensor that maps points from sensor
+   * frame to body frame. The position and orientation variables represent the body frame, and
+   * the extrinsic is applied internally to compute the sensor-frame pose for the prior.
+   *
+   * @param[in] source          The name of the sensor or motion model that generated this constraint
+   * @param[in] position        The variable representing the body-frame position
+   * @param[in] orientation     The variable representing the body-frame orientation
+   * @param[in] ext_position    The extrinsic translation (body-to-sensor)
+   * @param[in] ext_orientation The extrinsic rotation (body-to-sensor)
+   * @param[in] mean            The measured/prior sensor-frame pose (7x1 vector: x, y, z, qw, qx, qy, qz)
+   * @param[in] covariance      The measurement/prior covariance (6x6 matrix: x, y, z, qx, qy, qz)
+   */
+  AbsolutePose3DStampedConstraint(const std::string& source, const vesta_variables::Position3DStamped& position,
+                                  const vesta_variables::Orientation3DStamped& orientation,
+                                  const vesta_variables::Extrinsic3DPosition& ext_position,
+                                  const vesta_variables::Extrinsic3DOrientation& ext_orientation,
+                                  const vesta_core::Vector7d& mean, const vesta_core::Matrix6d& covariance);
+
+  /**
+   * @brief Returns whether this constraint uses an extrinsic calibration.
+   */
+  bool hasExtrinsic() const
+  {
+    return has_extrinsic_;
+  }
+
   /**
    * @brief Destructor
    */
@@ -155,6 +186,7 @@ class AbsolutePose3DStampedConstraint : public vesta_core::Constraint
 protected:
   vesta_core::Vector7d mean_;              //!< The measured/prior mean vector for this variable
   vesta_core::Matrix6d sqrt_information_;  //!< The square root information matrix
+  bool has_extrinsic_{ false };            //!< Whether this constraint uses an extrinsic calibration
 
 private:
   // Allow Boost Serialization access to private methods
@@ -175,6 +207,7 @@ class AbsolutePose3DStampedConstraint : public vesta_core::Constraint
     archive& boost::serialization::base_object<vesta_core::Constraint>(*this);
     archive & mean_;
     archive & sqrt_information_;
+    archive & has_extrinsic_;
   }
 };
 

vesta_constraints/include/vesta_constraints/3d/extrinsic_pose_3d.h

@@ -0,0 +1,75 @@
+#pragma once
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2026, Vesta Contributors
+ *  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.
+ */
+
+#include <ceres/rotation.h>
+
+namespace vesta_constraints
+{
+
+/**
+ * @brief Compute the sensor-frame pose in the world frame given a body pose and an extrinsic.
+ *
+ * The extrinsic T_body_sensor transforms points from the sensor frame to the body frame:
+ *   p_body = q_bs * p_sensor + t_bs
+ *
+ * Given the body pose in the world frame (p_wb, q_wb), the sensor pose in the world frame is:
+ *   q_ws = q_wb * q_bs
+ *   p_ws = p_wb + R_wb * t_bs
+ *
+ * @param[in]  body_position      Body position in world frame (3 elements: x, y, z)
+ * @param[in]  body_orientation   Body orientation quaternion in world frame (4 elements: w, x, y, z)
+ * @param[in]  ext_position       Extrinsic translation body-to-sensor (3 elements: x, y, z)
+ * @param[in]  ext_orientation    Extrinsic rotation quaternion body-to-sensor (4 elements: w, x, y, z)
+ * @param[out] sensor_position    Sensor position in world frame (3 elements)
+ * @param[out] sensor_orientation Sensor orientation quaternion in world frame (4 elements)
+ */
+template <typename T>
+inline void computeSensorPose(const T* const body_position, const T* const body_orientation,
+                              const T* const ext_position, const T* const ext_orientation, T* sensor_position,
+                              T* sensor_orientation)
+{
+  // q_ws = q_wb * q_bs
+  ceres::QuaternionProduct(body_orientation, ext_orientation, sensor_orientation);
+
+  // p_ws = p_wb + R_wb * t_bs
+  T rotated_ext_position[3];
+  ceres::QuaternionRotatePoint(body_orientation, ext_position, rotated_ext_position);
+  sensor_position[0] = body_position[0] + rotated_ext_position[0];
+  sensor_position[1] = body_position[1] + rotated_ext_position[1];
+  sensor_position[2] = body_position[2] + rotated_ext_position[2];
+}
+
+}  // namespace vesta_constraints

vesta_constraints/include/vesta_constraints/3d/normal_delta_orientation_3d_with_extrinsic_cost_functor.h

@@ -0,0 +1,115 @@
+#pragma once
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2026, Vesta Contributors
+ *  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.
+ */
+
+#include <vesta_core/eigen.h>
+#include <vesta_core/fuse_macros.h>
+
+#include <ceres/rotation.h>
+#include <Eigen/Core>
+
+namespace vesta_constraints
+{
+
+/**
+ * @brief Cost function for the difference between two 3D orientations with an extrinsic calibration.
+ *
+ * This extends NormalDeltaOrientation3DCostFunctor to accept body-frame orientation variables
+ * and an extrinsic transform. The sensor orientations are computed as q_sensor = q_body * q_ext,
+ * and the delta orientation residual is computed on the sensor orientations.
+ *
+ * The ext_position parameter block is present for graph consistency but is unused in the math,
+ * since translation does not affect orientation.
+ *
+ * Parameter blocks: body_orientation1 (4), body_orientation2 (4), ext_position (3), ext_orientation (4)
+ *
+ * Residuals: 3 (qx, qy, qz)
+ */
+class NormalDeltaOrientation3DWithExtrinsicCostFunctor
+{
+public:
+  VESTA_MAKE_ALIGNED_OPERATOR_NEW();
+
+  /**
+   * @brief Construct a cost function instance
+   *
+   * @param[in] A The residual weighting matrix (3x3), typically the square root information matrix
+   * @param[in] b The measured orientation change as a quaternion (4x1: w, x, y, z)
+   */
+  NormalDeltaOrientation3DWithExtrinsicCostFunctor(const vesta_core::Matrix3d& A, const vesta_core::Vector4d& b)
+    : A_(A), b_(b)
+  {
+  }
+
+  /**
+   * @brief Evaluate the cost function. Used by the Ceres optimization engine.
+   */
+  template <typename T>
+  bool operator()(const T* const body_orientation1, const T* const body_orientation2,
+                  const T* const /* ext_position */, const T* const ext_orientation, T* residuals) const
+  {
+    // Compute sensor orientations: q_sensor = q_body * q_ext
+    T sensor_orientation1[4];
+    ceres::QuaternionProduct(body_orientation1, ext_orientation, sensor_orientation1);
+
+    T sensor_orientation2[4];
+    ceres::QuaternionProduct(body_orientation2, ext_orientation, sensor_orientation2);
+
+    // Compute the delta: q1^-1 * q2
+    T orientation1_inverse[4] = { sensor_orientation1[0], -sensor_orientation1[1], -sensor_orientation1[2],
+                                  -sensor_orientation1[3] };
+
+    T observation_inverse[4] = { T(b_(0)), T(-b_(1)), T(-b_(2)), T(-b_(3)) };
+
+    T difference[4];
+    ceres::QuaternionProduct(orientation1_inverse, sensor_orientation2, difference);
+    T error[4];
+    ceres::QuaternionProduct(observation_inverse, difference, error);
+    ceres::QuaternionToAngleAxis(error, residuals);
+
+    // 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;
+  }
+
+private:
+  vesta_core::Matrix3d A_;  //!< The residual weighting matrix
+  vesta_core::Vector4d b_;  //!< The measured difference between orientation1 and orientation2
+};
+
+}  // namespace vesta_constraints