Issue258 add fixed size optimizer

fuse_core/include/fuse_core/parameter.h

@@ -171,6 +171,54 @@ inline fuse_core::Loss::SharedPtr loadLossConfig(const ros::NodeHandle& nh, cons
   return loss;
 }
 
+/**
+ * @brief Checks if a class has a loadFromROS method defined
+ */
+template <typename ParameterType>
+class has_load_from_ros
+{
+private:
+  template<typename T, T> struct helper;
+
+  template<typename T>
+  static std::true_type has_load_method(helper<void (T::*)(const ros::NodeHandle &), &T::loadFromROS>*);
+
+  template<typename T>
+  static std::false_type has_load_method(...);
+
+public:
+  static constexpr bool value = std::is_same<std::true_type, decltype(has_load_method<ParameterType>(nullptr))>::value;
+};
+
+/**
+ * @brief Template overload for parameter loading that takes a node handle. Calls a class loadFromROS method.
+ * @param[in] nh - The namespace from which to load the parameter
+ * @param[out] params - The parameter to fill with data from the ROS node handle
+ */
+template< typename ParameterType >
+void loadFromROS(const ros::NodeHandle &nh, ParameterType &params)
+{
+  static_assert(
+    has_load_from_ros<ParameterType>::value,
+    "loadFromROS() free function called on a type that does not implement the "
+    "loadFromROS(ros::NodeHandle ros_namespace, ParameterType params) method, nor implements a specialization of the "
+    "loadFromROS(ros_namespace) free function.");
+  params.loadFromROS(nh);
+}
+
+/**
+ * @brief Template overload for parameter loading that takes a node handle. Calls a class loadFromROS method.
+ * @param[in] nh - The ROS node handle with which to load the params
+ * @return The parameters, after being loaded from ROS
+ */
+template< typename ParameterType >
+ParameterType loadFromROS(const ros::NodeHandle &nh)
+{
+  ParameterType params;
+  params.loadFromROS(nh);
+  return params;
+}
+
 }  // namespace fuse_core
 
 #endif  // FUSE_CORE_PARAMETER_H

fuse_optimizers/CMakeLists.txt

@@ -12,6 +12,8 @@ set(build_depends
   std_srvs
 )
 
+find_package(OpenMP)
+
 find_package(catkin REQUIRED COMPONENTS
   ${build_depends}
 )
@@ -34,7 +36,9 @@ add_compile_options(-Wall -Werror)
 add_library(${PROJECT_NAME}
   src/batch_optimizer.cpp
   src/fixed_lag_smoother.cpp
+  src/fixed_size_smoother.cpp
   src/optimizer.cpp
+  src/windowed_optimizer.cpp
   src/variable_stamp_index.cpp
 )
 add_dependencies(${PROJECT_NAME}
@@ -47,12 +51,16 @@ target_include_directories(${PROJECT_NAME}
 )
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
+  ${OpenMP_CXX_LIBRARIES}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
     CXX_STANDARD 14
     CXX_STANDARD_REQUIRED YES
 )
+target_compile_options(${PROJECT_NAME}
+  PRIVATE ${OpenMP_CXX_FLAGS}
+)
 
 ## batch_optimizer node
 add_executable(batch_optimizer_node
@@ -98,6 +106,28 @@ set_target_properties(fixed_lag_smoother_node
     CXX_STANDARD_REQUIRED YES
 )
 
+## fixed_size_smoother node
+add_executable(fixed_size_smoother_node
+  src/fixed_size_smoother_node.cpp
+)
+add_dependencies(fixed_size_smoother_node
+  ${catkin_EXPORTED_TARGETS}
+)
+target_include_directories(fixed_size_smoother_node
+  PRIVATE
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+target_link_libraries(fixed_size_smoother_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+set_target_properties(fixed_size_smoother_node
+  PROPERTIES
+    CXX_STANDARD 14
+    CXX_STANDARD_REQUIRED YES
+)
+
 #############
 ## Install ##
 #############

fuse_optimizers/include/fuse_optimizers/batch_optimizer.h

@@ -51,10 +51,8 @@
 #include <utility>
 #include <vector>
 
-
 namespace fuse_optimizers
 {
-
 /**
  * @brief A simple optimizer implementation that uses batch optimization
  *
@@ -107,10 +105,8 @@ class BatchOptimizer : public Optimizer
    * @param[in] node_handle         A node handle in the global namespace
    * @param[in] private_node_handle A node handle in the node's private namespace
    */
-  BatchOptimizer(
-    fuse_core::Graph::UniquePtr graph,
-    const ros::NodeHandle& node_handle = ros::NodeHandle(),
-    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
+  BatchOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(),
+                 const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
 
   /**
    * @brief Destructor
@@ -126,11 +122,10 @@ class BatchOptimizer : public Optimizer
     std::string sensor_name;
     fuse_core::Transaction::SharedPtr transaction;
 
-    TransactionQueueElement(
-      const std::string& sensor_name,
-      fuse_core::Transaction::SharedPtr transaction) :
-        sensor_name(sensor_name),
-        transaction(std::move(transaction)) {}
+    TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction)
+      : sensor_name(sensor_name), transaction(std::move(transaction))
+    {
+    }
   };
 
   /**
@@ -145,19 +140,19 @@ class BatchOptimizer : public Optimizer
   fuse_core::Transaction::SharedPtr combined_transaction_;  //!< Transaction used aggregate constraints and variables
                                                             //!< from multiple sensors and motions models before being
                                                             //!< applied to the graph.
-  std::mutex combined_transaction_mutex_;  //!< Synchronize access to the combined transaction across different threads
-  ParameterType params_;  //!< Configuration settings for this optimizer
+  std::mutex combined_transaction_mutex_;   //!< Synchronize access to the combined transaction across different threads
+  ParameterType params_;                    //!< Configuration settings for this optimizer
   std::atomic<bool> optimization_request_;  //!< Flag to trigger a new optimization
   std::condition_variable optimization_requested_;  //!< Condition variable used by the optimization thread to wait
                                                     //!< until a new optimization is requested by the main thread
-  std::mutex optimization_requested_mutex_;  //!< Required condition variable mutex
-  std::thread optimization_thread_;  //!< Thread used to run the optimizer as a background process
-  ros::Timer optimize_timer_;  //!< Trigger an optimization operation at a fixed frequency
+  std::mutex optimization_requested_mutex_;         //!< Required condition variable mutex
+  std::thread optimization_thread_;                 //!< Thread used to run the optimizer as a background process
+  ros::Timer optimize_timer_;                       //!< Trigger an optimization operation at a fixed frequency
   TransactionQueue pending_transactions_;  //!< The set of received transactions that have not been added to the
                                            //!< optimizer yet. Transactions are added by the main thread, and removed
                                            //!< and processed by the optimization thread.
   std::mutex pending_transactions_mutex_;  //!< Synchronize modification of the pending_transactions_ container
-  ros::Time start_time_;  //!< The timestamp of the first ignition sensor transaction
+  ros::Time start_time_;                   //!< The timestamp of the first ignition sensor transaction
   bool started_;  //!< Flag indicating the optimizer is ready/has received a transaction from an ignition sensor
 
   /**
@@ -200,9 +195,7 @@ class BatchOptimizer : public Optimizer
    *                        to generate connected constraints.
    * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin
    */
-  void transactionCallback(
-    const std::string& sensor_name,
-    fuse_core::Transaction::SharedPtr transaction) override;
+  void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override;
 
   /**
    * @brief Update and publish diagnotics

fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h

@@ -45,10 +45,8 @@
 #include <string>
 #include <vector>
 
-
 namespace fuse_optimizers
 {
-
 /**
  * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class
  */
@@ -62,15 +60,15 @@ struct BatchOptimizerParams
    * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency"
    * parameter in Hz.
    */
-  ros::Duration optimization_period { 0.1 };
+  ros::Duration optimization_period{ 0.1 };
 
   /**
    * @brief The maximum time to wait for motion models to be generated for a received transaction.
    *
    * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for
    * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue.
    */
-  ros::Duration transaction_timeout { 0.1 };
+  ros::Duration transaction_timeout{ 0.1 };
 
   /**
    * @brief Ceres Solver::Options object that controls various aspects of the optimizer.