locusrobotics · svwilliams · Jul 4, 2023
diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp
@@ -34,11 +34,9 @@
 #ifndef FUSE_VARIABLES__POINT_2D_FIXED_LANDMARK_HPP_
 #define FUSE_VARIABLES__POINT_2D_FIXED_LANDMARK_HPP_
 
-#include <ostream>
-
 #include <fuse_core/fuse_macros.hpp>
 #include <fuse_core/serialization.hpp>
-#include <fuse_variables/fixed_size_variable.hpp>
+#include <fuse_variables/point_2d_landmark.hpp>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
@@ -50,24 +48,16 @@ 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
    */
@@ -80,47 +70,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 in to/out of the
@@ -132,8 +89,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);
   }
 };
 

diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp
@@ -38,6 +38,7 @@
 
 #include <fuse_core/fuse_macros.hpp>
 #include <fuse_core/serialization.hpp>
+#include <fuse_core/uuid.hpp>
 #include <fuse_variables/fixed_size_variable.hpp>
 
 #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;

diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp
@@ -34,11 +34,9 @@
 #ifndef FUSE_VARIABLES__POINT_3D_FIXED_LANDMARK_HPP_
 #define FUSE_VARIABLES__POINT_3D_FIXED_LANDMARK_HPP_
 
-#include <ostream>
-
 #include <fuse_core/fuse_macros.hpp>
 #include <fuse_core/serialization.hpp>
-#include <fuse_variables/fixed_size_variable.hpp>
+#include <fuse_variables/point_3d_landmark.hpp>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
@@ -50,25 +48,16 @@ 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
    */
@@ -81,57 +70,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 in to/out of the
@@ -143,8 +89,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);
   }
 };
 

diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp
@@ -41,6 +41,7 @@
 
 #include <fuse_core/fuse_macros.hpp>
 #include <fuse_core/serialization.hpp>
+#include <fuse_core/uuid.hpp>
 #include <fuse_variables/fixed_size_variable.hpp>
 
 #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;

diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp
@@ -31,37 +31,18 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#include <ostream>
-
 #include <boost/serialization/export.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_core/variable.hpp>
-#include <fuse_variables/fixed_size_variable.hpp>
 #include <fuse_variables/point_2d_fixed_landmark.hpp>
+#include <fuse_variables/point_2d_landmark.hpp>
 #include <pluginlib/class_list_macros.hpp>
 
 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

diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp
@@ -42,12 +42,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"

diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp
@@ -31,38 +31,18 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#include <ostream>
-
 #include <boost/serialization/export.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_core/variable.hpp>
-#include <fuse_variables/fixed_size_variable.hpp>
 #include <fuse_variables/point_3d_fixed_landmark.hpp>
+#include <fuse_variables/point_3d_landmark.hpp>
 #include <pluginlib/class_list_macros.hpp>
 
 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

diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp
@@ -42,12 +42,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"