[RST-2340] Add serialization support to fuse (#98)

locusrobotics · Aug 26, 2019 · a425641 · a425641
1 parent c4a3a62
commit a425641
Show file tree

Hide file tree

Showing 117 changed files with 4,285 additions and 319 deletions.
diff --git a/fuse/package.xml b/fuse/package.xml
@@ -15,6 +15,7 @@
   <exec_depend>fuse_core</exec_depend>
   <exec_depend>fuse_graphs</exec_depend>
   <exec_depend>fuse_models</exec_depend>
+  <exec_depend>fuse_msgs</exec_depend>
   <exec_depend>fuse_optimizers</exec_depend>
   <exec_depend>fuse_publishers</exec_depend>
   <exec_depend>fuse_variables</exec_depend>

diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt
@@ -6,6 +6,7 @@ set(build_depends
   fuse_graphs
   fuse_variables
   geometry_msgs
+  pluginlib
   roscpp
 )
 
@@ -39,6 +40,7 @@ add_compile_options(-std=c++14 -Wall -Werror)
 
 # fuse_constraints library
 add_library(${PROJECT_NAME}
+  src/absolute_constraint.cpp
   src/absolute_orientation_3d_stamped_constraint.cpp
   src/absolute_orientation_3d_stamped_euler_constraint.cpp
   src/absolute_pose_2d_stamped_constraint.cpp
@@ -49,6 +51,7 @@ add_library(${PROJECT_NAME}
   src/normal_delta.cpp
   src/normal_delta_orientation_2d.cpp
   src/normal_prior_orientation_2d.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
@@ -88,6 +91,11 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
+install(
+  FILES fuse_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
 #############
 ## Testing ##
 #############

diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml
@@ -0,0 +1,121 @@
+<library path="lib/libfuse_constraints">
+  <class type="fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 2D angular acceleration, or a direct measurement of
+    the 2D 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::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
+    2D orientation.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsolutePosition2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 2D position, or a direct measurement of the
+    2D position.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsolutePosition3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D position, or a direct measurement of the
+    3D position.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 2D angular velocity, or a direct measurement of
+    the 2D 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::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
+    3D orientation.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D orientation, or a direct measurement of the
+    3D orientation as roll-pitch-yaw Euler angles.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsolutePose2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 2D pose, or a direct measurement of the 2D pose.
+    </description>
+  </class>
+  <class type="fuse_constraints::AbsolutePose3DStampedConstraint" 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.
+    </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.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeAccelerationAngular2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D angular acceleration variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeAccelerationLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D linear acceleration variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeOrientation2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D orientation variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativePosition2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D position variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativePosition3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 3D position variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeVelocityAngular2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D angular velocity variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeVelocityLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 2D linear velocity variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativeOrientation3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between 3D orientation variables.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativePose2DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between two 2D poses.
+    </description>
+  </class>
+  <class type="fuse_constraints::RelativePose3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between two 3D poses.
+    </description>
+  </class>
+</library>
diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_constraint.h
@@ -37,6 +37,7 @@
 #include <fuse_core/constraint.h>
 #include <fuse_core/eigen.h>
 #include <fuse_core/macros.h>
+#include <fuse_core/serialization.h>
 #include <fuse_core/uuid.h>
 #include <fuse_variables/acceleration_angular_2d_stamped.h>
 #include <fuse_variables/acceleration_linear_2d_stamped.h>
@@ -46,6 +47,9 @@
 #include <fuse_variables/velocity_angular_2d_stamped.h>
 #include <fuse_variables/velocity_linear_2d_stamped.h>
 
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
 #include <ceres/cost_function.h>
 
 #include <ostream>
@@ -64,11 +68,16 @@ namespace fuse_constraints
  * This constraint holds the measured variable value and the measurement uncertainty/covariance.
  */
 template<class Variable>
-class AbsoluteConstraint final : public fuse_core::Constraint
+class AbsoluteConstraint : public fuse_core::Constraint
 {
 public:
   FUSE_CONSTRAINT_DEFINITIONS(AbsoluteConstraint<Variable>);
 
+  /**
+   * @brief Default constructor
+   */
+  AbsoluteConstraint() = default;
+
   /**
    * @brief Create a constraint using a measurement/prior of all dimensions of the target variable
    *
@@ -149,6 +158,24 @@ class AbsoluteConstraint final : public fuse_core::Constraint
 protected:
   fuse_core::VectorXd 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_;
+  }
 };
 
 // Define unique names for the different variations of the absolute constraint
@@ -164,4 +191,12 @@ using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint<fuse_variab
 // 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::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);
+
 #endif  // FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_H
diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.h b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.h
@@ -39,6 +39,7 @@
 #include <ceres/normal_prior.h>
 #include <Eigen/Dense>
 
+#include <string>
 #include <vector>
 
 
@@ -108,8 +109,8 @@ void AbsoluteConstraint<Variable>::print(std::ostream& stream) const
 {
   stream << type() << "\n"
          << "  uuid: " << uuid() << "\n"
-         << "  variable: " << variables_.at(0) << "\n"
-         << "  mean: " << mean_.transpose() << "\n"
+         << "  variable: " << variables().at(0) << "\n"
+         << "  mean: " << mean().transpose() << "\n"
          << "  sqrt_info: " << sqrtInformation() << "\n";
 }
 
@@ -128,6 +129,49 @@ inline ceres::CostFunction* AbsoluteConstraint<fuse_variables::Orientation2DStam
   return new NormalPriorOrientation2D(sqrt_information_(0, 0), mean_(0));
 }
 
+// Specialize the type() method to return the name that is registered with the plugins
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::Orientation2DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteOrientation2DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::Position2DStamped>::type() const
+{
+  return "fuse_constraints::AbsolutePosition2DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::Position3DStamped>::type() const
+{
+  return "fuse_constraints::AbsolutePosition3DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint";
+}
+
+template<>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint";
+}
+
 }  // namespace fuse_constraints
 
 #endif  // FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_IMPL_H
diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h
@@ -37,11 +37,15 @@
 #include <fuse_core/constraint.h>
 #include <fuse_core/eigen.h>
 #include <fuse_core/macros.h>
+#include <fuse_core/serialization.h>
 #include <fuse_core/uuid.h>
 #include <fuse_variables/orientation_3d_stamped.h>
 #include <geometry_msgs/PoseWithCovariance.h>
 #include <geometry_msgs/Quaternion.h>
 
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
 #include <Eigen/Geometry>
 
 #include <array>
@@ -62,6 +66,11 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint
 public:
   FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsoluteOrientation3DStampedConstraint);
 
+  /**
+   * @brief Default constructor
+   */
+  AbsoluteOrientation3DStampedConstraint() = default;
+
   /**
    * @brief Create a constraint using a measurement/prior of a 3D orientation
    *
@@ -166,8 +175,28 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint
 
   fuse_core::Vector4d mean_;  //!< The measured/prior mean vector for this variable
   fuse_core::Matrix3d 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::AbsoluteOrientation3DStampedConstraint);
+
 #endif  // FUSE_CONSTRAINTS_ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H