Address PR comments

locusrobotics · Feb 14, 2022 · 74876a6 · 74876a6
1 parent ca47b2e
commit 74876a6
Show file tree

Hide file tree

Showing 24 changed files with 407 additions and 505 deletions.
diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt
@@ -1,8 +1,6 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(fuse_optimizers)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
-
 set(build_depends
   diagnostic_updater
   fuse_constraints
@@ -14,6 +12,8 @@ set(build_depends
   std_srvs
 )
 
+find_package(OpenMP)
+
 find_package(catkin REQUIRED COMPONENTS
   ${build_depends}
 )
@@ -51,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

diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/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

diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/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.

diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
@@ -113,11 +113,9 @@ class FixedLagSmoother : public WindowedOptimizer
    * @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
    */
-  FixedLagSmoother(
-    fuse_core::Graph::UniquePtr graph,
-    const ParameterType::SharedPtr& params,
-    const ros::NodeHandle& node_handle = ros::NodeHandle(),
-    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
+  FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params,
+                   const ros::NodeHandle& node_handle = ros::NodeHandle(),
+                   const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
 
   /**
    * @brief Constructor
@@ -129,18 +127,16 @@ class FixedLagSmoother : public WindowedOptimizer
    * @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
    */
-  explicit FixedLagSmoother(
-    fuse_core::Graph::UniquePtr graph,
-    const ros::NodeHandle& node_handle = ros::NodeHandle(),
-    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
+  explicit FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(),
+                            const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
 
 protected:
   // Read-only after construction
   ParameterType::SharedPtr params_;  //!< Configuration settings for this fixed-lag smoother
 
   // Guarded by mutex_
-  std::mutex mutex_;  //!< Mutex held while the fixed-lag smoother variables are modified
-  ros::Time lag_expiration_;  //!< The oldest stamp that is inside the fixed-lag smoother window
+  std::mutex mutex_;                       //!< Mutex held while the fixed-lag smoother variables are modified
+  ros::Time lag_expiration_;               //!< The oldest stamp that is inside the fixed-lag smoother window
   VariableStampIndex timestamp_tracking_;  //!< Object that tracks the timestamp associated with each variable
 
   /**

diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
@@ -59,7 +59,7 @@ struct FixedLagSmootherParams : public WindowedOptimizerParams
   /**
    * @brief The duration of the smoothing window in seconds
    */
-  ros::Duration lag_duration { 5.0 };
+  ros::Duration lag_duration{ 5.0 };
 
   /**
    * @brief Method for loading parameter values from ROS.

diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h
@@ -1,7 +1,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2019, Locus Robotics
+ *  Copyright (c) 2022, Locus Robotics
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -50,17 +50,17 @@
 namespace fuse_optimizers
 {
 /**
- * @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time
+ * @brief A fixed-2ize smoother implementation that marginalizes out variables that are older than a defined buffer size
  *
  * This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class,
  * or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of
- * the fixed-Size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables.
+ * the fixed-size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables.
  *
  * During optimization:
  *  (1) new variables and constraints are added to the graph
  *  (2) the augmented graph is optimized and the variable values are updated
  *  (3) all motion models, sensors, and publishers are notified of the updated graph
- *  (4) all variables older than "current time - Size duration" are marginalized out.
+ *  (4) all variables outside of the fixed buffer are marginalized out
  *
  * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received
  * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the
@@ -69,7 +69,7 @@ namespace fuse_optimizers
  * completion, and the next optimization will not begin until the next scheduled optimization period.
  *
  * Parameters:
- *  - Size_duration (float, default: 5.0) The duration of the smoothing window in seconds
+ *  - num_states (float, default: 10) The number of unique timestamped states in the window
  *  - motion_models (struct array) The set of motion model plugins to load
  *    @code{.yaml}
  *    - name: string  (A unique name for this motion model)
@@ -113,11 +113,9 @@ class FixedSizeSmoother : public WindowedOptimizer
    * @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
    */
-  FixedSizeSmoother(
-    fuse_core::Graph::UniquePtr graph,
-    const ParameterType::SharedPtr& params,
-    const ros::NodeHandle& node_handle = ros::NodeHandle(),
-    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
+  FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params,
+                    const ros::NodeHandle& node_handle = ros::NodeHandle(),
+                    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
 
   /**
    * @brief Constructor
@@ -129,17 +127,15 @@ class FixedSizeSmoother : public WindowedOptimizer
    * @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
    */
-  explicit FixedSizeSmoother(
-    fuse_core::Graph::UniquePtr graph,
-    const ros::NodeHandle& node_handle = ros::NodeHandle(),
-    const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
+  explicit FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(),
+                             const ros::NodeHandle& private_node_handle = ros::NodeHandle("~"));
 
 protected:
   // Read-only after construction
   ParameterType::SharedPtr params_;  //!< Configuration settings for this fixed-Size smoother
 
   // Guarded by mutex_
-  std::mutex mutex_;  //!< Mutex held while the fixed-size smoother variables are modified
+  std::mutex mutex_;                       //!< Mutex held while the fixed-size smoother variables are modified
   VariableStampIndex timestamp_tracking_;  //!< Object that tracks the timestamp associated with each variable
 
   /**

diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h
@@ -1,7 +1,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2019, Locus Robotics
+ *  Copyright (c) 2022, Locus Robotics
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -34,18 +34,10 @@
 #ifndef FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H
 #define FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H
 
-#include <fuse_core/ceres_options.h>
 #include <fuse_core/parameter.h>
 #include <fuse_optimizers/windowed_optimizer_params.h>
-#include <ros/duration.h>
 #include <ros/node_handle.h>
 
-#include <ceres/solver.h>
-
-#include <algorithm>
-#include <string>
-#include <vector>
-
 namespace fuse_optimizers
 {
 /**
@@ -57,9 +49,9 @@ struct FixedSizeSmootherParams : public WindowedOptimizerParams
   SMART_PTR_DEFINITIONS(FixedSizeSmootherParams);
 
   /**
-   * @brief The duration of the smoothing window in seconds
+   * @brief Thenumber of unique stamps in the window
    */
-  int num_states {10};
+  int num_states{ 10 };
 
   /**
    * @brief Method for loading parameter values from ROS.