Make 2D versions of the landmark variables

fuse_variables/CMakeLists.txt

@@ -36,6 +36,8 @@ add_library(${PROJECT_NAME}
   src/acceleration_linear_3d_stamped.cpp
   src/orientation_2d_stamped.cpp
   src/orientation_3d_stamped.cpp
+  src/point_2d_fixed_landmark.cpp
+  src/point_2d_landmark.cpp
   src/point_3d_fixed_landmark.cpp
   src/point_3d_landmark.cpp
   src/position_2d_stamped.cpp
@@ -289,49 +291,49 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD_REQUIRED YES
   )
 
-  # Position 2D Stamped Tests
-  catkin_add_gtest(test_position_2d_stamped
-    test/test_position_2d_stamped.cpp
+  # Point 2D fixed landmark tests
+  catkin_add_gtest(test_point_2d_fixed_landmark
+    test/test_point_2d_fixed_landmark.cpp
   )
-  add_dependencies(test_position_2d_stamped
+  add_dependencies(test_point_2d_fixed_landmark
     ${catkin_EXPORTED_TARGETS}
   )
-  target_include_directories(test_position_2d_stamped
+  target_include_directories(test_point_2d_fixed_landmark
     PRIVATE
       include
       ${catkin_INCLUDE_DIRS}
       ${CERES_INCLUDE_DIRS}
   )
-  target_link_libraries(test_position_2d_stamped
+  target_link_libraries(test_point_2d_fixed_landmark
     ${PROJECT_NAME}
     ${catkin_LIBRARIES}
     ${CERES_LIBRARIES}
   )
-  set_target_properties(test_position_2d_stamped
+  set_target_properties(test_point_2d_fixed_landmark
     PROPERTIES
       CXX_STANDARD 14
       CXX_STANDARD_REQUIRED YES
   )
 
-  # Position 3D stamped tests
-  catkin_add_gtest(test_position_3d_stamped
-    test/test_position_3d_stamped.cpp
+  # Point 2D landmark tests
+  catkin_add_gtest(test_point_2d_landmark
+    test/test_point_2d_landmark.cpp
   )
-  add_dependencies(test_position_3d_stamped
+  add_dependencies(test_point_2d_landmark
     ${catkin_EXPORTED_TARGETS}
   )
-  target_include_directories(test_position_3d_stamped
+  target_include_directories(test_point_2d_landmark
     PRIVATE
       include
       ${catkin_INCLUDE_DIRS}
       ${CERES_INCLUDE_DIRS}
   )
-  target_link_libraries(test_position_3d_stamped
+  target_link_libraries(test_point_2d_landmark
     ${PROJECT_NAME}
     ${catkin_LIBRARIES}
     ${CERES_LIBRARIES}
   )
-  set_target_properties(test_position_3d_stamped
+  set_target_properties(test_point_2d_landmark
     PROPERTIES
       CXX_STANDARD 14
       CXX_STANDARD_REQUIRED YES
@@ -385,6 +387,54 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD_REQUIRED YES
   )
 
+  # Position 2D Stamped Tests
+  catkin_add_gtest(test_position_2d_stamped
+    test/test_position_2d_stamped.cpp
+  )
+  add_dependencies(test_position_2d_stamped
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_position_2d_stamped
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_position_2d_stamped
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_position_2d_stamped
+    PROPERTIES
+      CXX_STANDARD 14
+      CXX_STANDARD_REQUIRED YES
+  )
+
+  # Position 3D stamped tests
+  catkin_add_gtest(test_position_3d_stamped
+    test/test_position_3d_stamped.cpp
+  )
+  add_dependencies(test_position_3d_stamped
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_position_3d_stamped
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_position_3d_stamped
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_position_3d_stamped
+    PROPERTIES
+      CXX_STANDARD 14
+      CXX_STANDARD_REQUIRED YES
+  )
+
   # Velocity Angular 2D Stamped Tests
   catkin_add_gtest(test_velocity_angular_2d_stamped
     test/test_velocity_angular_2d_stamped.cpp

fuse_variables/fuse_plugins.xml

@@ -31,6 +31,28 @@
     hardware.
     </description>
   </class>
+  <class type="fuse_variables::Point2DLandmark" base_class_type="fuse_core::Variable">
+    <description>
+    Variable representing a 2D point landmark that exists across time.
+    </description>
+  </class>
+  <class type="fuse_variables::Point2DFixedLandmark" base_class_type="fuse_core::Variable">
+    <description>
+    Variable representing a 2D point landmark that exists across time. This version is assumed to be known perfectly,
+    and will not be updated during optimization.
+    </description>
+  </class>
+  <class type="fuse_variables::Point3DLandmark" base_class_type="fuse_core::Variable">
+    <description>
+    Variable representing a 3D point landmark that exists across time.
+    </description>
+  </class>
+  <class type="fuse_variables::Point3DFixedLandmark" base_class_type="fuse_core::Variable">
+    <description>
+    Variable representing a 3D point landmark that exists across time. This version is assumed to be known perfectly,
+    and will not be updated during optimization.
+    </description>
+  </class>
   <class type="fuse_variables::Position2DStamped" base_class_type="fuse_core::Variable">
     <description>
     Variable representing a 2D position (x, y) at a specific time, with a specific piece of hardware.

fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h

@@ -0,0 +1,146 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, 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_VARIABLES_POINT_2D_FIXED_LANDMARK_H
+#define FUSE_VARIABLES_POINT_2D_FIXED_LANDMARK_H
+
+#include <fuse_core/macros.h>
+#include <fuse_core/serialization.h>
+#include <fuse_variables/fixed_size_variable.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.
+ */
+class Point2DFixedLandmark : public FixedSizeVariable<2>
+{
+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
+   */
+  Point2DFixedLandmark() = default;
+
+  /**
+   * @brief Construct a point 2D variable given a landmarks id
+   *
+   * @param[in] landmark_id  The id associated to a landmark
+   */
+  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;
+
+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 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<FixedSizeVariable<SIZE>>(*this);
+    archive& id_;
+  }
+};
+
+}  // namespace fuse_variables
+
+BOOST_CLASS_EXPORT_KEY(fuse_variables::Point2DFixedLandmark);
+
+#endif  // FUSE_VARIABLES_POINT_2D_FIXED_LANDMARK_H