Derive the fixed landmarks from the standard landmarks (#259)

Author: svwilliams

fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h

@@ -36,37 +36,27 @@
 
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/serialization.h>
-#include <fuse_variables/fixed_size_variable.h>
+#include <fuse_variables/point_2d_landmark.h>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <ostream>
-
 namespace fuse_variables
 {
 /**
  * @brief Variable representing a 2D point landmark that exists across time.
  *
- * This is commonly used to represent locations of visual features. The UUID of this class is constant after
- * construction and dependent on a user input database id. As such, the database id cannot be altered after
- * construction.
+ * This is commonly used to represent locations of visual features. This class differs from the Point2DLandmark in that
+ * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are
+ * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and
+ * dependent on a user input database id. As such, the database id cannot be altered after construction.
  */
-class Point2DFixedLandmark : public FixedSizeVariable<2>
+class Point2DFixedLandmark : public Point2DLandmark
 {
 public:
   FUSE_VARIABLE_DEFINITIONS(Point2DFixedLandmark);
 
-  /**
-   * @brief Can be used to directly index variables in the data array
-   */
-  enum : size_t
-  {
-    X = 0,
-    Y = 1
-  };
-
   /**
    * @brief Default constructor
    */
@@ -79,48 +69,14 @@ class Point2DFixedLandmark : public FixedSizeVariable<2>
    */
   explicit Point2DFixedLandmark(const uint64_t& landmark_id);
 
-  /**
-   * @brief Read-write access to the X-axis position.
-   */
-  double& x() { return data_[X]; }
-
-  /**
-   * @brief Read-only access to the X-axis position.
-   */
-  const double& x() const { return data_[X]; }
-
-  /**
-   * @brief Read-write access to the Y-axis position.
-   */
-  double& y() { return data_[Y]; }
-
-  /**
-   * @brief Read-only access to the Y-axis position.
-   */
-  const double& y() const { return data_[Y]; }
-
-  /**
-   * @brief Read-only access to the id
-   */
-  const uint64_t& id() const { return id_; }
-
-  /**
-   * @brief Print a human-readable description of the variable 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 Specifies if the value of the variable should not be changed during optimization
    */
-  bool holdConstant() const override;
+  bool holdConstant() const override { return true; }
 
 private:
   // Allow Boost Serialization access to private methods
   friend class boost::serialization::access;
-  uint64_t id_ { 0 };
 
   /**
    * @brief The Boost Serialize method that serializes all of the data members
@@ -134,8 +90,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2>
   template <class Archive>
   void serialize(Archive& archive, const unsigned int /* version */)
   {
-    archive& boost::serialization::base_object<FixedSizeVariable<SIZE>>(*this);
-    archive& id_;
+    archive& boost::serialization::base_object<Point2DLandmark>(*this);
   }
 };
 

fuse_variables/include/fuse_variables/point_2d_landmark.h

@@ -36,6 +36,7 @@
 
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/serialization.h>
+#include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 
 #include <boost/serialization/access.hpp>
@@ -112,6 +113,15 @@ class Point2DLandmark : public FixedSizeVariable<2>
    */
   void print(std::ostream& stream = std::cout) const override;
 
+protected:
+  /**
+   * @brief Construct a point 2D variable given a UUID and a landmarks id
+   *
+   * @param[in] uuid  The UUID for this variable
+   * @param[in] landmark_id  The id associated to a landmark
+   */
+  Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id);
+
 private:
   // Allow Boost Serialization access to private methods
   friend class boost::serialization::access;

fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h

@@ -36,38 +36,27 @@
 
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/serialization.h>
-#include <fuse_variables/fixed_size_variable.h>
+#include <fuse_variables/point_3d_landmark.h>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/export.hpp>
 
-#include <ostream>
-
 namespace fuse_variables
 {
 /**
  * @brief Variable representing a 3D point landmark that exists across time.
  *
- * This is commonly used to represent locations of visual features. The UUID of this class is constant after
- * construction and dependent on a user input database id. As such, the database id cannot be altered after
- * construction.
+ * This is commonly used to represent locations of visual features. This class differs from the Point3DLandmark in that
+ * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are
+ * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and
+ * dependent on a user input database id. As such, the database id cannot be altered after construction.
  */
-class Point3DFixedLandmark : public FixedSizeVariable<3>
+class Point3DFixedLandmark : public Point3DLandmark
 {
 public:
   FUSE_VARIABLE_DEFINITIONS(Point3DFixedLandmark);
 
-  /**
-   * @brief Can be used to directly index variables in the data array
-   */
-  enum : size_t
-  {
-    X = 0,
-    Y = 1,
-    Z = 2
-  };
-
   /**
    * @brief Default constructor
    */
@@ -80,58 +69,14 @@ class Point3DFixedLandmark : public FixedSizeVariable<3>
    */
   explicit Point3DFixedLandmark(const uint64_t& landmark_id);
 
-  /**
-   * @brief Read-write access to the X-axis position.
-   */
-  double& x() { return data_[X]; }
-
-  /**
-   * @brief Read-only access to the X-axis position.
-   */
-  const double& x() const { return data_[X]; }
-
-  /**
-   * @brief Read-write access to the Y-axis position.
-   */
-  double& y() { return data_[Y]; }
-
-  /**
-   * @brief Read-only access to the Y-axis position.
-   */
-  const double& y() const { return data_[Y]; }
-
-  /**
-   * @brief Read-write access to the Z-axis position.
-   */
-  double& z() { return data_[Z]; }
-
-  /**
-   * @brief Read-only access to the Z-axis position.
-   */
-  const double& z() const { return data_[Z]; }
-
-  /**
-   * @brief Read-only access to the id
-   */
-  const uint64_t& id() const { return id_; }
-
-  /**
-   * @brief Print a human-readable description of the variable 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 Specifies if the value of the variable should not be changed during optimization
    */
-  bool holdConstant() const override;
+  bool holdConstant() const override { return true; }
 
 private:
   // Allow Boost Serialization access to private methods
   friend class boost::serialization::access;
-  uint64_t id_ { 0 };
 
   /**
    * @brief The Boost Serialize method that serializes all of the data members
@@ -145,8 +90,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3>
   template <class Archive>
   void serialize(Archive& archive, const unsigned int /* version */)
   {
-    archive& boost::serialization::base_object<FixedSizeVariable<SIZE>>(*this);
-    archive& id_;
+    archive& boost::serialization::base_object<Point3DLandmark>(*this);
   }
 };
 

fuse_variables/include/fuse_variables/point_3d_landmark.h

@@ -39,6 +39,7 @@
 
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/serialization.h>
+#include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 
 #include <boost/serialization/access.hpp>
@@ -126,6 +127,14 @@ class Point3DLandmark : public FixedSizeVariable<3>
    */
   void print(std::ostream& stream = std::cout) const override;
 
+protected:
+  /**
+   * @brief Construct a point 3D variable given a landmarks id
+   *
+   * @param[in] landmark_id  The id associated to a landmark
+   */
+  Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id);
+
 private:
   // Allow Boost Serialization access to private methods
   friend class boost::serialization::access;

fuse_variables/src/point_2d_fixed_landmark.cpp

@@ -35,35 +35,16 @@
 
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <fuse_variables/fixed_size_variable.h>
+#include <fuse_variables/point_2d_landmark.h>
 #include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
-#include <ostream>
-
 namespace fuse_variables
 {
 Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) :
-  FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
-  id_(landmark_id)
-{
-}
-
-void Point2DFixedLandmark::print(std::ostream& stream) const
-{
-  stream << type() << ":\n"
-         << "  uuid: " << uuid() << "\n"
-         << "  size: " << size() << "\n"
-         << "  landmark id: " << id() << "\n"
-         << "  data:\n"
-         << "  - x: " << x() << "\n"
-         << "  - y: " << y() << "\n";
-}
-
-bool Point2DFixedLandmark::holdConstant() const
+  Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
 {
-  return true;
 }
 
 }  // namespace fuse_variables

fuse_variables/src/point_2d_landmark.cpp

@@ -44,12 +44,17 @@
 
 namespace fuse_variables
 {
-Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) :
-  FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
+Point2DLandmark::Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) :
+  FixedSizeVariable(uuid),
   id_(landmark_id)
 {
 }
 
+Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) :
+  Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
+{
+}
+
 void Point2DLandmark::print(std::ostream& stream) const
 {
   stream << type() << ":\n"

fuse_variables/src/point_3d_fixed_landmark.cpp

@@ -35,36 +35,16 @@
 
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <fuse_variables/fixed_size_variable.h>
+#include <fuse_variables/point_3d_landmark.h>
 #include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
-#include <ostream>
-
 namespace fuse_variables
 {
 Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) :
-  FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
-  id_(landmark_id)
-{
-}
-
-void Point3DFixedLandmark::print(std::ostream& stream) const
-{
-  stream << type() << ":\n"
-         << "  uuid: " << uuid() << "\n"
-         << "  size: " << size() << "\n"
-         << "  landmark id: " << id() << "\n"
-         << "  data:\n"
-         << "  - x: " << x() << "\n"
-         << "  - y: " << y() << "\n"
-         << "  - z: " << z() << "\n";
-}
-
-bool Point3DFixedLandmark::holdConstant() const
+  Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
 {
-  return true;
 }
 
 }  // namespace fuse_variables

fuse_variables/src/point_3d_landmark.cpp

@@ -44,12 +44,17 @@
 
 namespace fuse_variables
 {
-Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) :
-  FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
+Point3DLandmark::Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) :
+  FixedSizeVariable(uuid),
   id_(landmark_id)
 {
 }
 
+Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) :
+    Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
+{
+}
+
 void Point3DLandmark::print(std::ostream& stream) const
 {
   stream << type() << ":\n"