3D fuse models

fuse.repos

@@ -0,0 +1,9 @@
+repositories:
+  locusrobotics/tf2_2d:
+    type: git
+    url: https://github.com/locusrobotics/tf2_2d.git
+    version: rolling
+  covariance_geometry_ros:
+    type: git
+    url: https://github.com/giafranchini/covariance_geometry_ros.git
+    version: iron

fuse_constraints/CMakeLists.txt

@@ -37,18 +37,23 @@ add_library(${PROJECT_NAME}
   src/absolute_orientation_3d_stamped_euler_constraint.cpp
   src/absolute_pose_2d_stamped_constraint.cpp
   src/absolute_pose_3d_stamped_constraint.cpp
+  src/absolute_pose_3d_stamped_euler_constraint.cpp
   src/marginal_constraint.cpp
   src/marginal_cost_function.cpp
   src/marginalize_variables.cpp
   src/normal_delta.cpp
   src/normal_delta_orientation_2d.cpp
   src/normal_delta_pose_2d.cpp
   src/normal_prior_orientation_2d.cpp
+  src/normal_prior_orientation_3d.cpp
   src/normal_prior_pose_2d.cpp
+  src/normal_prior_pose_3d.cpp
+  src/normal_prior_pose_3d_euler.cpp
   src/relative_constraint.cpp
   src/relative_orientation_3d_stamped_constraint.cpp
   src/relative_pose_2d_stamped_constraint.cpp
   src/relative_pose_3d_stamped_constraint.cpp
+  src/relative_pose_3d_stamped_euler_constraint.cpp
   src/uuid_ordering.cpp
   src/variable_constraints.cpp
 )

fuse_constraints/fuse_plugins.xml

@@ -5,12 +5,24 @@
     the 2D angular acceleration.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of
+    the 3D angular acceleration.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D linear acceleration, or a direct measurement of
     the 2D linear acceleration.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of
+    the 3D linear acceleration.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteOrientation2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D orientation, or a direct measurement of the
@@ -35,12 +47,24 @@
     the 2D angular velocity.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of
+    the 3D angular velocity.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D linear velocity, or a direct measurement of
     the 2D linear velocity.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of
+    the 3D linear velocity.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteOrientation3DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 3D orientation, or a direct measurement of the
@@ -63,6 +87,12 @@
     A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose.
     </description>
   </class>
+  <class type="fuse_constraints::AbsolutePose3DStampedEulerConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose,
+    orientation parametrized in RPY.
+    </description>
+  </class>
   <class type="fuse_constraints::MarginalConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents remaining marginal information on a set of variables.
@@ -118,4 +148,9 @@
     A constraint that represents a measurement on the difference between two 3D poses.
     </description>
   </class>
+  <class type="fuse_constraints::RelativePose3DStampedEulerConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between two 3D poses, orientation parametrized in RPY.
+    </description>
+  </class>
 </library>

fuse_constraints/include/fuse_constraints/absolute_constraint.hpp

@@ -46,11 +46,15 @@
 #include <fuse_core/serialization.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_variables/acceleration_angular_2d_stamped.hpp>
+#include <fuse_variables/acceleration_angular_3d_stamped.hpp>
 #include <fuse_variables/acceleration_linear_2d_stamped.hpp>
+#include <fuse_variables/acceleration_linear_3d_stamped.hpp>
 #include <fuse_variables/orientation_2d_stamped.hpp>
 #include <fuse_variables/position_2d_stamped.hpp>
 #include <fuse_variables/position_3d_stamped.hpp>
 #include <fuse_variables/velocity_angular_2d_stamped.hpp>
+#include <fuse_variables/velocity_angular_3d_stamped.hpp>
+#include <fuse_variables/velocity_linear_3d_stamped.hpp>
 #include <fuse_variables/velocity_linear_2d_stamped.hpp>
 
 #include <boost/serialization/access.hpp>
@@ -196,27 +200,39 @@ class AbsoluteConstraint : public fuse_core::Constraint
 // Define unique names for the different variations of the absolute constraint
 using AbsoluteAccelerationAngular2DStampedConstraint =
   AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>;
+using AbsoluteAccelerationAngular3DStampedConstraint =
+  AbsoluteConstraint<fuse_variables::AccelerationAngular3DStamped>;
 using AbsoluteAccelerationLinear2DStampedConstraint =
   AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>;
+using AbsoluteAccelerationLinear3DStampedConstraint = 
+  AbsoluteConstraint<fuse_variables::AccelerationLinear3DStamped>;
 using AbsoluteOrientation2DStampedConstraint =
   AbsoluteConstraint<fuse_variables::Orientation2DStamped>;
 using AbsolutePosition2DStampedConstraint = AbsoluteConstraint<fuse_variables::Position2DStamped>;
 using AbsolutePosition3DStampedConstraint = AbsoluteConstraint<fuse_variables::Position3DStamped>;
 using AbsoluteVelocityAngular2DStampedConstraint =
   AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>;
+using AbsoluteVelocityAngular3DStampedConstraint =
+  AbsoluteConstraint<fuse_variables::VelocityAngular3DStamped>;
 using AbsoluteVelocityLinear2DStampedConstraint =
   AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>;
+using AbsoluteVelocityLinear3DStampedConstraint =
+  AbsoluteConstraint<fuse_variables::VelocityLinear3DStamped>;
 }  // namespace fuse_constraints
 
 // Include the template implementation
 #include <fuse_constraints/absolute_constraint_impl.hpp>
 
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteOrientation2DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint);
 
 #endif  // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_

fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp

@@ -155,6 +155,18 @@ inline std::string AbsoluteConstraint<fuse_variables::AccelerationLinear2DStampe
   return "fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint";
 }
 
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationAngular3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationLinear3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint";
+}
+
 template<>
 inline std::string AbsoluteConstraint<fuse_variables::Orientation2DStamped>::type() const
 {
@@ -185,6 +197,18 @@ inline std::string AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>::
   return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint";
 }
 
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityAngular3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityLinear3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint";
+}
+
 }  // namespace fuse_constraints
 
 #endif  // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_

fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp

@@ -0,0 +1,194 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Giacomo Franchini
+ *  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__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_
+#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_
+
+#include <Eigen/Dense>
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+#include <fuse_core/constraint.hpp>
+#include <fuse_core/eigen.hpp>
+#include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/serialization.hpp>
+#include <fuse_core/uuid.hpp>
+#include <fuse_variables/orientation_3d_stamped.hpp>
+#include <fuse_variables/position_3d_stamped.hpp>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+
+
+namespace fuse_constraints
+{
+
+/**
+ * @brief A constraint that represents either prior information about a 3D pose, or a direct
+ *        measurement of the 3D pose. 
+ *
+ * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience,
+ * this class applies an absolute constraint on both variables at once. This type of constraint
+ * arises in many situations. In mapping it is common to define the very first pose as the origin.
+ * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame.
+ * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map
+ * measurements). This constraint holds the measured 3D pose: orientation is parametrized as roll-pitch-yaw
+ * Euler angles and the covariance represents the error around each translational and rotational axis.
+ * This constraint allows measurement of a subset of the pose components given in the variable.
+ */
+
+class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint
+{
+public:
+  FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint)
+
+  /**
+   * @brief Default constructor
+   */
+  AbsolutePose3DStampedEulerConstraint() = default;
+
+  /**
+   * @brief Create a constraint using a measurement/prior of the 3D pose
+   *
+   * @param[in] source          The name of the sensor or motion model that generated this constraint
+   * @param[in] position        The variable representing the position components of the pose
+   * @param[in] orientation     The variable representing the orientation components of the pose
+   * @param[in] mean            The measured/prior pose as a vector
+   *                            (6x1 vector: x, y, z, roll, pitch, yaw)
+   * @param[in] covariance      The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw)
+   */
+  AbsolutePose3DStampedEulerConstraint(
+    const std::string & source,
+    const fuse_variables::Position3DStamped & position,
+    const fuse_variables::Orientation3DStamped & orientation,
+    const fuse_core::Vector6d & mean,
+    const fuse_core::Matrix6d & covariance);
+
+  /**
+   * @brief Create a constraint using a partial measurement/prior of the 3D pose
+   *
+   * @param[in] source          The name of the sensor or motion model that generated this constraint
+   * @param[in] position        The variable representing the position components of the pose
+   * @param[in] orientation     The variable representing the orientation components of the pose
+   * @param[in] partial_mean    The measured/prior pose as a vector
+   *                            (6x1 vector: x, y, z, roll, pitch, yaw)
+   * @param[in] partial_covariance  The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw)
+   * @param[in] variable_indices    The indices of the measured variables
+   */
+  AbsolutePose3DStampedEulerConstraint(
+    const std::string & source,
+    const fuse_variables::Position3DStamped & position,
+    const fuse_variables::Orientation3DStamped & orientation,
+    const fuse_core::Vector6d & partial_mean,
+    const fuse_core::MatrixXd & partial_covariance,
+    const std::vector<size_t> & variable_indices);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~AbsolutePose3DStampedEulerConstraint() = default;
+
+  /**
+   * @brief Read-only access to the measured/prior vector of mean values.
+   *
+   * Order is (x, y, z, roll, pitch, yaw)
+   */
+  const fuse_core::Vector6d & mean() const {return mean_;}
+
+  /**
+   * @brief Read-only access to the square root information matrix.
+   *
+   * Order is (x, y, z, roll, pitch, yaw)
+   */
+  const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;}
+
+  /**
+   * @brief Compute the measurement covariance matrix.
+   *
+   * Order is (x, y, z, roll, pitch, yaw)
+   */
+  fuse_core::Matrix6d covariance() const;
+
+  /**
+   * @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 Construct an instance of this constraint's cost function
+   *
+   * 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::Vector6d mean_;  //!< The measured/prior mean vector for this variable
+  fuse_core::MatrixXd sqrt_information_;  //!< The square root information matrix
+
+private:
+  // Allow Boost Serialization access to private methods
+  friend class boost::serialization::access;
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members in to/out of the
+   *        archive
+   *
+   * @param[in/out] archive - The archive object that holds the serialized class members
+   * @param[in] version - The version of the archive being read/written. Generally unused.
+   */
+  template<class Archive>
+  void serialize(Archive & archive, const unsigned int /* version */)
+  {
+    archive & boost::serialization::base_object<fuse_core::Constraint>(*this);
+    archive & mean_;
+    archive & sqrt_information_;
+  }
+};
+
+}  // namespace fuse_constraints
+
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint);
+
+#endif  // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_