Integrate 3D models

fuse_constraints/CMakeLists.txt

@@ -44,6 +44,7 @@ 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/fixed_3d_landmark_constraint.cpp
   src/marginal_constraint.cpp
   src/marginal_cost_function.cpp

fuse_constraints/fuse_plugins.xml

@@ -11,6 +11,18 @@
     the 2D linear 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::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
@@ -41,6 +53,18 @@
     the 2D linear 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::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,
+    with the 3D orientation as roll-pitch-yaw Euler angles.
+    </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.

fuse_constraints/include/fuse_constraints/absolute_constraint.h

@@ -41,11 +41,15 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/acceleration_angular_2d_stamped.h>
 #include <fuse_variables/acceleration_linear_2d_stamped.h>
+#include <fuse_variables/acceleration_angular_3d_stamped.h>
+#include <fuse_variables/acceleration_linear_3d_stamped.h>
 #include <fuse_variables/orientation_2d_stamped.h>
 #include <fuse_variables/position_2d_stamped.h>
 #include <fuse_variables/position_3d_stamped.h>
 #include <fuse_variables/velocity_angular_2d_stamped.h>
 #include <fuse_variables/velocity_linear_2d_stamped.h>
+#include <fuse_variables/velocity_angular_3d_stamped.h>
+#include <fuse_variables/velocity_linear_3d_stamped.h>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
@@ -186,22 +190,30 @@ class AbsoluteConstraint : public fuse_core::Constraint
 // Define unique names for the different variations of the absolute constraint
 using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>;
 using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>;
+using AbsoluteAccelerationAngular3DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular3DStamped>;
+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 AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>;
+using AbsoluteVelocityAngular3DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityAngular3DStamped>;
+using AbsoluteVelocityLinear3DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear3DStamped>;
 }  // namespace fuse_constraints
 
 // Include the template implementation
 #include <fuse_constraints/absolute_constraint_impl.h>
 
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint);
+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::AbsoluteVelocityLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint);
 
 #endif  // FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_H

fuse_constraints/include/fuse_constraints/absolute_constraint_impl.h

@@ -151,6 +151,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
 {
@@ -181,6 +193,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_H

fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.h

@@ -0,0 +1,165 @@
+/*
+ * 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_ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_H
+#define FUSE_CONSTRAINTS_ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_H
+
+#include <fuse_core/constraint.h>
+#include <fuse_core/eigen.h>
+#include <fuse_core/fuse_macros.h>
+#include <fuse_core/serialization.h>
+#include <fuse_core/uuid.h>
+#include <fuse_variables/orientation_3d_stamped.h>
+#include <fuse_variables/position_3d_stamped.h>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+#include <Eigen/Dense>
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+
+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 and the measurement
+ * uncertainty/covariance. Orientations are represented as roll, pitch, yaw.
+ */
+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 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::Matrix6d& sqrtInformation() const { return sqrt_information_; }
+
+  /**
+   * @brief Compute the measurement covariance matrix.
+   *
+   * Order is (x, y, z, roll, pitch, yaw)
+   */
+  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 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::Matrix6d 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_H

fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h

@@ -55,7 +55,7 @@ namespace fuse_constraints
  *
  * So, for example, if
  * b_ = [ measured_yaw  ]
- *      [ meausred_roll ]
+ *      [ measured_roll ]
  *
  * then the cost function is of the form:
  *
@@ -140,7 +140,7 @@ class NormalPriorOrientation3DEulerCostFunctor
 
 private:
   fuse_core::MatrixXd A_;  //!< The residual weighting matrix, most likely the square root information matrix
-  fuse_core::VectorXd b_;  //!< The measured 3D orientation (quaternion) value
+  fuse_core::VectorXd b_;  //!< The orientation measurement or prior. Its order must match the values in \p axes.
   std::vector<Euler> axes_;  //!< The Euler angle axes that we're measuring
 };