fuse -> ROS 2 fuse_graphs : Port fuse_graphs

fuse_core/CMakeLists.txt

@@ -107,7 +107,7 @@ install(DIRECTORY include/
 
 ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET)
 ament_export_dependencies(
-  ament_cmake
+  ament_cmake_ros
   fuse_msgs
   pluginlib
   rcl_interfaces

fuse_core/include/fuse_core/graph.hpp

@@ -143,8 +143,8 @@ namespace fuse_core
  *        that form the factor graph, a graphical model of a nonlinear least-squares problem.
  *
  * Methods are provided to add, remove, and access the constraints and variables by several
- * criteria, as well as to optimize the variable values. Derived classes may store the
- * constraints and variables using any mechanism, but the same interface must be provided.
+ * criteria, as well as to optimize the variable values. Derived classes may store the constraints
+ * and variables using any mechanism, but the same interface must be provided.
  */
 class Graph
 {
@@ -155,19 +155,19 @@ class Graph
    * @brief A range of fuse_core::Constraint objects
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods
-   * (which means it can be used in range-based for loops), an empty() method, and a front()
-   * method for directly accessing the first member. When dereferenced, an iterator returns a
-   * const Constraint&.
+   * (which means it can be used in range-based for loops), an empty() method, and a front() method
+   * for directly accessing the first member. When dereferenced, an iterator returns a const
+   * Constraint&.
    */
   using const_constraint_range = boost::any_range<const Constraint, boost::forward_traversal_tag>;
 
   /**
    * @brief A range of fuse_core::Variable objects
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods
-   * (which means it can be used in range-based for loops), an empty() method, and a front()
-   * method for directly accessing the first member. When dereferenced, an iterator returns a
-   * const Variable&.
+   * (which means it can be used in range-based for loops), an empty() method, and a front() method
+   * for directly accessing the first member. When dereferenced, an iterator returns a const
+   * Variable&.
    */
   using const_variable_range = boost::any_range<const Variable, boost::forward_traversal_tag>;
 
@@ -185,16 +185,15 @@ class Graph
   /**
    * @brief Returns a unique name for this graph type.
    *
-   * The constraint type string must be unique for each class. As such, the fully-qualified
-   * class name is an excellent choice for the type string.
+   * The constraint type string must be unique for each class. As such, the fully-qualified class
+   * name is an excellent choice for the type string.
    */
   virtual std::string type() const = 0;
 
   /**
    * @brief Clear all variables and constraints from the graph object.
    *
-   * The object should be equivalent to a newly constructed object after clear() has been
-   * called.
+   * The object should be equivalent to a newly constructed object after clear() has been called.
    */
   virtual void clear() = 0;
 
@@ -216,9 +215,9 @@ class Graph
   /**
    * @brief Add a new constraint to the graph
    *
-   * Any referenced variables must exist in the graph before the constraint is added. The Graph
-   * will share ownership of the constraint. If this constraint already exists in the graph,
-   * the function will return false.
+   * Any referenced variables must exist in the graph before the constraint is added. The Graph will
+   * share ownership of the constraint. If this constraint already exists in the graph, the function
+   * will return false.
    *
    * @param[in] constraint The new constraint to be added
    * @return               True if the constraint was added, false otherwise
@@ -271,8 +270,8 @@ class Graph
   /**
    * @brief Add a new variable to the graph
    *
-   * The Graph will share ownership of the Variable. If this variable already exists in the
-   * graph, the function will return false.
+   * The Graph will share ownership of the Variable. If this variable already exists in the graph,
+   * the function will return false.
    *
    * @param[in] variable The new variable to be added
    * @return             True if the variable was added, false otherwise
@@ -318,8 +317,8 @@ class Graph
    * @brief Configure a variable to hold its current value constant during optimization
    *
    * Once set, the specified variable's value will no longer change during any subsequent
-   * optimization. To 'unhold' a previously held variable, call Graph::holdVariable() with the
-   * \p hold_constant parameter set to false.
+   * optimization. To 'unhold' a previously held variable, call Graph::holdVariable() with the \p
+   * hold_constant parameter set to false.
    *
    * @param[in] variable_uuid The variable to adjust
    * @param[in] hold_constant Flag indicating if the variable's value should be held constant during
@@ -339,11 +338,11 @@ class Graph
   /**
    * @brief Compute the marginal covariance blocks for the requested set of variable pairs.
    *
-   * To compute the marginal variance of a single variable, simply supply the same variable
-   * UUID for both members of of the request pair. Computing the marginal covariance is an
-   * expensive operation; grouping multiple variable pairs into a single call will be much
-   * faster than calling this function for each pair individually. The marginal covariances can
-   * only be computed after calling Graph::computeUpdates() or Graph::optimize().
+   * To compute the marginal variance of a single variable, simply supply the same variable UUID for
+   * both members of of the request pair. Computing the marginal covariance is an expensive
+   * operation; grouping multiple variable pairs into a single call will be much faster than calling
+   * this function for each pair individually. The marginal covariances can only be computed after
+   * calling Graph::computeUpdates() or Graph::optimize().
    *
    * @param[in]  covariance_requests A set of variable UUID pairs for which the marginal covariance
    *                                 is desired.
@@ -387,8 +386,8 @@ class Graph
    *        constraints for a maximum amount of time.
    *
    * The \p max_optimization_time should be viewed as a "best effort" limit, and the actual
-   * optimization time may exceed this limit by a small amount. After the call, the values in
-   * the graph will be updated to the latest values.
+   * optimization time may exceed this limit by a small amount. After the call, the values in the
+   * graph will be updated to the latest values.
    *
    * @param[in] max_optimization_time The maximum allowed duration of the optimization call
    * @param[in] options An optional Ceres Solver::Options object that controls various aspects of
@@ -399,7 +398,9 @@ class Graph
    */
   virtual ceres::Solver::Summary optimizeFor(
     const rclcpp::Duration & max_optimization_time,
-    const ceres::Solver::Options & options = ceres::Solver::Options()) = 0;
+    const ceres::Solver::Options & options = ceres::Solver::Options(),
+    rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0;  // NOTE(CH3): We need to copy
+                                                                // because clock.now() is non-const
 
   /**
    * @brief Evalute the values of the current set of variables, given the current set of
@@ -408,21 +409,20 @@ class Graph
    * The values in the graph do not change after the call.
    *
    * If any of the output arguments is nullptr, it will not be evaluated. This mimics the
-   * ceres::Problem::Evaluate method API. Here all output arguments default to nullptr except
-   * for the cost.
+   * ceres::Problem::Evaluate method API. Here all output arguments default to nullptr except for
+   * the cost.
    *
    * TODO(efernandez) support jacobian output argument
-   *                  The jacobian output argument is not exposed at the moment because its type is
-   *                  a CRSMatrix, that probably needs to be converted to another type.
+   * The jacobian output argument is not exposed at the moment because its type is a CRSMatrix, that
+   * probably needs to be converted to another type.
    *
    * @param[out] cost      The cost of the entire problem represented by the graph.
    * @param[out] residuals The residuals of all constraints.
    * @param[out] gradient  The gradient for all constraints evaluated at the values of the current
    *                       set of variables.
    * @param[in]  options   An optional Ceres Problem::EvaluateOptions object that controls various
    *                       aspects of the problem evaluation. See https://ceres-
-   *                       solver.googlesource.com/ceres-
-   *                       solver/+/master/include/ceres/problem.h#401
+   *                       solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401
    * @return True if the problem evaluation was successful; False, otherwise.
    */
   virtual bool evaluate(
@@ -520,9 +520,10 @@ class Graph
    * @brief The Boost Serialize method that serializes all of the data members in to/out of the
    *        archive
    *
-   * This method, or a combination of save() and load() methods, must be implemented by all
-   * derived classes. See documentation on Boost Serialization for information on how to
-   * implement the serialize() method.
+   * This method, or a combination of save() and load() methods, must be implemented by all derived
+   * classes. See documentation on Boost Serialization for information on how to implement the
+   * serialize() method.
+   *
    * https://www.boost.org/doc/libs/1_70_0/libs/serialization/doc/
    *
    * @param[in/out] archive - The archive object that holds the serialized class members

fuse_graphs/CMakeLists.txt

@@ -1,142 +1,91 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.16)
 project(fuse_graphs)
 
-set(build_depends
-  fuse_core
-  pluginlib
-  roscpp
-)
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CXX_STANDARD_REQUIRED YES)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(fuse_core REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  ${build_depends}
-)
-find_package(Boost REQUIRED)
 find_package(Ceres REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    ${PROJECT_NAME}
-  CATKIN_DEPENDS
-    ${build_depends}
-  DEPENDS
-    Boost
-    CERES
-)
+include(boost-extras.cmake)
 
 ###########
 ## Build ##
 ###########
-add_compile_options(-Wall -Werror)
 
 ## fuse_graphs library
 add_library(${PROJECT_NAME}
   src/hash_graph.cpp
 )
-add_dependencies(${PROJECT_NAME}
-  ${catkin_EXPORTED_TARGETS}
-)
-target_include_directories(${PROJECT_NAME}
-  PUBLIC
-    include
-    ${Boost_INCLUDE_DIRS}
-    ${catkin_INCLUDE_DIRS}
-    ${CERES_INCLUDE_DIRS}
-)
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${CERES_LIBRARIES}
-)
-set_target_properties(${PROJECT_NAME}
-  PROPERTIES
-    CXX_STANDARD 14
-    CXX_STANDARD_REQUIRED YES
-)
-
-#############
-## Install ##
-#############
-
-install(
-  TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-)
-
-install(
-  DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
-
-install(
-  FILES fuse_plugins.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Boost::serialization
+  Ceres::ceres
+  fuse_core::fuse_core
+  pluginlib::pluginlib
+  rclcpp::rclcpp
 )
 
 #############
 ## Testing ##
 #############
 
-if(CATKIN_ENABLE_TESTING)
-  find_package(roslint REQUIRED)
-  find_package(rostest REQUIRED)
-
-  # Lint tests
-  set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
-  roslint_cpp()
-  roslint_add_test()
-
-  # HashGraph tests
-  catkin_add_gtest(test_hash_graph
-    test/test_hash_graph.cpp
-  )
-  add_dependencies(test_hash_graph
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_hash_graph
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-  )
-  target_link_libraries(test_hash_graph
-    ${PROJECT_NAME}
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_hash_graph
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  add_subdirectory(test)
 
   # Benchmarks
   find_package(benchmark QUIET)
-
   if(benchmark_FOUND)
     # Create Problem benchmark
-    add_executable(benchmark_create_problem
-      benchmark/benchmark_create_problem.cpp
-    )
-    target_include_directories(benchmark_create_problem
-      PRIVATE
-        include
-        ${Boost_INCLUDE_DIRS}
-        ${catkin_INCLUDE_DIRS}
-        ${CMAKE_CURRENT_SOURCE_DIR}
-    )
+    add_executable(benchmark_create_problem benchmark/benchmark_create_problem.cpp)
+    target_include_directories(benchmark_create_problem PRIVATE "test/")
     target_link_libraries(benchmark_create_problem
-      benchmark
       ${PROJECT_NAME}
-      ${catkin_LIBRARIES}
-    )
-    set_target_properties(benchmark_create_problem
-      PROPERTIES
-        CXX_STANDARD 14
-        CXX_STANDARD_REQUIRED YES
+      benchmark::benchmark
     )
   endif()
 endif()
+
+#############
+## Install ##
+#############
+
+install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml)
+
+ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  ament_cmake_ros
+  fuse_core
+  pluginlib
+  rclcpp
+  Ceres
+)
+
+ament_package(CONFIG_EXTRAS boost-extras.cmake)