From 5de7bd432dfbce902c244fb886b7e20115e78e0c Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Mon, 9 Jan 2023 11:36:30 +0100 Subject: [PATCH 1/4] add initial version of step-wise fixed-lag smoother and perform necessary changes on normal fixed-lag smoother to re-use many parts and derive from it; fix build warnings in fuse_core/variable.h; fix typo in comment at batch_optimizer_params.h --- fuse_core/include/fuse_core/variable.h | 4 +- fuse_optimizers/CMakeLists.txt | 24 ++ .../fuse_optimizers/batch_optimizer_params.h | 2 +- .../fuse_optimizers/fixed_lag_smoother.h | 2 + .../step_wise_fixed_lag_smoother.h | 177 +++++++++++++ .../step_wise_fixed_lag_smoother_params.h | 105 ++++++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 15 +- .../src/fixed_lag_smoother_node.cpp | 1 + .../src/step_wise_fixed_lag_smoother.cpp | 238 ++++++++++++++++++ .../src/step_wise_fixed_lag_smoother_node.cpp | 51 ++++ 10 files changed, 610 insertions(+), 9 deletions(-) create mode 100644 fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h create mode 100644 fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h create mode 100644 fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp create mode 100644 fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h index 2b7252b14..22ee9b4ab 100644 --- a/fuse_core/include/fuse_core/variable.h +++ b/fuse_core/include/fuse_core/variable.h @@ -325,7 +325,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The lower bound for the requested variable dimension */ - virtual double lowerBound(size_t index) const + virtual double lowerBound(size_t /* index */) const { return std::numeric_limits::lowest(); } @@ -338,7 +338,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The upper bound for the requested variable dimension */ - virtual double upperBound(size_t index) const + virtual double upperBound(size_t /* index */) const { return std::numeric_limits::max(); } diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..55bf90843 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -34,6 +34,7 @@ add_compile_options(-Wall -Werror) add_library(${PROJECT_NAME} src/batch_optimizer.cpp src/fixed_lag_smoother.cpp + src/step_wise_fixed_lag_smoother.cpp src/optimizer.cpp src/variable_stamp_index.cpp ) @@ -98,6 +99,28 @@ set_target_properties(fixed_lag_smoother_node CXX_STANDARD_REQUIRED YES ) +## step_wise_fixed_lag_smoother node +add_executable(step_wise_fixed_lag_smoother_node + src/step_wise_fixed_lag_smoother_node.cpp +) +add_dependencies(step_wise_fixed_lag_smoother_node + ${catkin_EXPORTED_TARGETS} +) +target_include_directories(step_wise_fixed_lag_smoother_node + PRIVATE + include + ${catkin_INCLUDE_DIRS} +) +target_link_libraries(step_wise_fixed_lag_smoother_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(step_wise_fixed_lag_smoother_node + PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED YES +) + ############# ## Install ## ############# @@ -114,6 +137,7 @@ install( TARGETS batch_optimizer_node fixed_lag_smoother_node + step_wise_fixed_lag_smoother_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h index cc92ee4c8..9a3416bfb 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h @@ -50,7 +50,7 @@ namespace fuse_optimizers { /** - * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class + * @brief Defines the set of parameters required by the fuse_optimizers::BatchOptimizer class */ struct BatchOptimizerParams { diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index eefacac2a..fbd080f9f 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -126,6 +126,8 @@ class FixedLagSmoother : public Optimizer */ virtual ~FixedLagSmoother(); + virtual void startOptimization(); + protected: /** * Structure containing the information required to process a transaction after it was received. diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h new file mode 100644 index 000000000..8b5e05118 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h @@ -0,0 +1,177 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H +#define FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief A step-wise fixed-lag smoother implementation that marginalizes out variables that are older than a defined lag time + * and optimizes step-wise adding more and more constraints from different sources + * + * 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-lag smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. + * + * During optimization: + * (1) a set of new variables and constraints are computed + * (2) each optimization step adds the constraints with the defined source(s) to the graph + * (2) the augmented graph is optimized and the variable values are updated + * (3) steps 2 and 3 are continued until all optimization steps are completed + * (3) all motion models, sensors, and publishers are notified of the updated graph + * (4) all variables older than "current time - lag duration" 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 + * next optimization cycle. If the previous optimization is not yet complete when the optimization period elapses, + * then a warning will be logged but a new optimization will *not* be started. The previous optimization will run to + * completion, and the next optimization will not begin until the next scheduled optimization period. + * + * Parameters: + * - lag_duration (float, default: 5.0) The duration of the smoothing window in seconds + * - motion_models (struct array) The set of motion model plugins to load + * @code{.yaml} + * - name: string (A unique name for this motion model) + * type: string (The plugin loader class string for the desired motion model type) + * - ... + * @endcode + * - optimization_frequency (float, default: 10.0) The target frequency for optimization cycles. If an optimization + * takes longer than expected, an optimization cycle may be skipped. + * - publishers (struct array) The set of publisher plugins to load + * @code{.yaml} + * - name: string (A unique name for this publisher) + * type: string (The plugin loader class string for the desired publisher type) + * - ... + * @endcode + * - sensor_models (struct array) The set of sensor model plugins to load + * @code{.yaml} + * - name: string (A unique name for this sensor model) + * type: string (The plugin loader class string for the desired sensor model type) + * motion_models: [name1, name2, ...] (An optional list of motion model names that should be applied) + * - ... + * @endcode + * - transaction_timeout (float, default: 0.10) The maximum time to wait for motion models to be generated for a + * received transactions. Transactions are processes 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. + * - optimization_steps (struct array) The set of sources that should be optimized for each optimization step + * It is recommended to always add the ignition source(s) and motion model source(s) + * to the first optimization step. The constraints and variables of the marginalization + * are always added to the first optimization step. + * @code{.yaml} + * - sources: string array (A list of strings of the names of the sources of the constraints. + * The names refer to the names that are used in the other parameters sensor_models and motion_models) + * - ... + * @endcode + * full example: + * @code{.yaml} + * optimization_steps: + * - + * sources: + * - "initial_localization_sensor" + * - "unicycle_motion_model" + * - + * sources: + * - "odometry_sensor" + * - "imu_sensor" + * - + * sources: + * - "scan_to_scan" + * - + * sources: + * - "scan_to_map" + * @endcode + */ +class StepWiseFixedLagSmoother : public FixedLagSmoother +{ +public: + FUSE_SMART_PTR_DEFINITIONS(StepWiseFixedLagSmoother); + using ParameterType = StepWiseFixedLagSmootherParams; + + /** + * @brief Constructor + * + * @param[in] graph The derived graph object. This allows different graph implementations to be used + * with the same optimizer code. + * @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 + */ + StepWiseFixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + + /** + * @brief Destructor + */ + virtual ~StepWiseFixedLagSmoother(); + + /** + * @brief Function that optimizes all constraints, designed to be run in a separate thread. + * + * This function waits for an optimization or shutdown signal, then either calls optimize() or exits appropriately. + */ + void optimizationLoop(); + + void startOptimization() override; + +protected: + + // Read-only after construction + ParameterType params_; //!< Configuration settings for this fixed-lag smoother + +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h new file mode 100644 index 000000000..0b8ced5df --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H +#define FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief Defines the set of parameters required by the fuse_optimizers::StepWiseFixedLagSmoother class + */ +struct StepWiseFixedLagSmootherParams : FixedLagSmootherParams +{ +public: + + /** + * @brief The ordered list of steps for the optimization with each source inside + * + * The optimization is performed sequentially where each step adds more constraints from different sources to the graph. + */ + std::vector> optimization_steps; + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) + { + FixedLagSmootherParams::loadFromROS(nh); + + // load optimization steps + XmlRpc::XmlRpcValue optimization_steps_raw; + fuse_core::getParamRequired(nh, "optimization_steps", optimization_steps_raw); + + ROS_ASSERT(optimization_steps_raw.getType() == XmlRpc::XmlRpcValue::TypeArray); + + for (int step_i = 0; step_i < optimization_steps_raw.size(); ++step_i) + { + ROS_ASSERT(optimization_steps_raw[step_i].getType() == XmlRpc::XmlRpcValue::TypeStruct); + + ROS_ASSERT(optimization_steps_raw[step_i]["sources"].getType() == XmlRpc::XmlRpcValue::TypeArray); + + std::vector optimization_step_sources; + + for (int source_i = 0; source_i < optimization_steps_raw[step_i]["sources"].size(); ++source_i) + { + ROS_ASSERT(optimization_steps_raw[step_i]["sources"][source_i].getType() == XmlRpc::XmlRpcValue::TypeString); + + optimization_step_sources.push_back(static_cast(optimization_steps_raw[step_i]["sources"][source_i])); + } + + optimization_steps.push_back(optimization_step_sources); + } + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..d1e6dff0b 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -93,6 +93,15 @@ FixedLagSmoother::FixedLagSmoother( // Test for auto-start autostart(); + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = node_handle_.advertiseService( + ros::names::resolve(params_.reset_service), + &FixedLagSmoother::resetServiceCallback, + this); +} + +void FixedLagSmoother::startOptimization() +{ // Start the optimization thread optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); @@ -101,12 +110,6 @@ FixedLagSmoother::FixedLagSmoother( params_.optimization_period, &FixedLagSmoother::optimizerTimerCallback, this); - - // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_.reset_service), - &FixedLagSmoother::resetServiceCallback, - this); } FixedLagSmoother::~FixedLagSmoother() diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 25c8230fb..bdf34dc3a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -44,6 +44,7 @@ int main(int argc, char **argv) fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(private_node_handle); fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + optimizer.startOptimization(); ros::spin(); return 0; diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp new file mode 100644 index 000000000..d29c07254 --- /dev/null +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp @@ -0,0 +1,238 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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. + */ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace fuse_optimizers +{ + +StepWiseFixedLagSmoother::StepWiseFixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) : + fuse_optimizers::FixedLagSmoother(std::move(graph), node_handle, private_node_handle) +{ + params_.loadFromROS(private_node_handle); + + // add its own marginal constraints source to the optimization steps + if (params_.optimization_steps.empty()) + { + params_.optimization_steps.push_back({ros::this_node::getName()}); + } + else + { + params_.optimization_steps[0].push_back(ros::this_node::getName()); + } + + // Start the optimization thread + // optimization_thread_ = std::thread(&StepWiseFixedLagSmoother::optimizationLoop, this); +} + +void StepWiseFixedLagSmoother::startOptimization() +{ + // Start the optimization thread + optimization_thread_ = std::thread(&StepWiseFixedLagSmoother::optimizationLoop, this); + + // Configure a timer to trigger optimizations + optimize_timer_ = node_handle_.createTimer( + params_.optimization_period, + &StepWiseFixedLagSmoother::optimizerTimerCallback, + this); +} + +StepWiseFixedLagSmoother::~StepWiseFixedLagSmoother() +{ + // Wake up any sleeping threads + optimization_running_ = false; + optimization_requested_.notify_all(); + // Wait for the threads to shutdown + if (optimization_thread_.joinable()) + { + optimization_thread_.join(); + } +} + +void StepWiseFixedLagSmoother::optimizationLoop() +{ + auto exit_wait_condition = [this]() + { + return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); + }; + // Optimize constraints until told to exit + while (ros::ok() && optimization_running_) + { + // Wait for the next signal to start the next optimization cycle + auto optimization_deadline = ros::Time(0, 0); + { + std::unique_lock lock(optimization_requested_mutex_); + optimization_requested_.wait(lock, exit_wait_condition); + optimization_request_ = false; + optimization_deadline = optimization_deadline_; + } + // If a shutdown is requested, exit now. + if (!optimization_running_ || !ros::ok()) + { + break; + } + // Optimize + { + std::lock_guard lock(optimization_mutex_); + // Apply motion models + auto new_transaction = fuse_core::Transaction::make_shared(); + // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ + // We do this to ensure state of the graph does not change between unlocking the pending_transactions + // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are + // not extremely careful, we could get a deadlock. + processQueue(*new_transaction, lag_expiration_); + // Skip this optimization cycle if the transaction is empty because something failed while processing the pending + // transactions queue. + if (new_transaction->empty()) + { + continue; + } + // Prepare for selecting the marginal variables + preprocessMarginalization(*new_transaction); + // Combine the new transactions with any marginal transaction from the end of the last cycle + new_transaction->merge(marginal_transaction_); + + ROS_DEBUG_STREAM("New transaction for all optimization steps: " << *new_transaction); + + for (auto &&types : params_.optimization_steps) + { + ROS_DEBUG_STREAM("optimization_step"); + for (auto &&type : types) + { + ROS_DEBUG_STREAM("type: " << type); + } + + fuse_core::Transaction::SharedPtr new_transaction_for_loop = new_transaction->clone(); + + // Keep only the sensor models that will be optimized in this loop + std::unordered_set constraints_for_removal; + for (const auto& constraint : new_transaction_for_loop->addedConstraints()) + { + if (std::find(types.begin(), types.end(), constraint.source()) == types.end()) + { + constraints_for_removal.insert(constraint.uuid()); + } + } + + // remove all constraints marked for removal + for (auto &&i : constraints_for_removal) + { + new_transaction_for_loop->removeConstraint(i); + } + + // skip if there are no elements for optimization in this optimization step + if (boost::empty(new_transaction_for_loop->addedConstraints()) || new_transaction_for_loop->empty()) + { + ROS_DEBUG_STREAM("Skipping because no elements in transaction"); + continue; + } + + ROS_DEBUG_STREAM("New transaction within optimization step: " << *new_transaction_for_loop); + + // Update the graph + try + { + graph_->update(*new_transaction_for_loop); + } + catch (const std::exception& ex) + { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction_for_loop->print(oss); + + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" << oss.str()); + ros::requestShutdown(); + break; + } + // Optimize the entire graph + summary_ = graph_->optimize(params_.solver_options); + + // Optimization is complete. Notify all the things about the graph changes. + const auto new_transaction_stamp = new_transaction_for_loop->stamp(); + notify(std::move(new_transaction_for_loop), graph_->clone()); + + // Abort if optimization failed. Not converging is not a failure because the solution found is usable. + if (!summary_.IsSolutionUsable()) + { + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_INFO_STREAM(summary_.FullReport()); + ros::requestShutdown(); + break; + } + } + + // Compute a transaction that marginalizes out those variables. + lag_expiration_ = computeLagExpirationTime(); + marginal_transaction_ = fuse_constraints::marginalizeVariables( + ros::this_node::getName(), + computeVariablesToMarginalize(lag_expiration_), + *graph_); + // Perform any post-marginal cleanup + postprocessMarginalization(marginal_transaction_); + // Note: The marginal transaction will not be applied until the next optimization iteration + // Log a warning if the optimization took too long + auto optimization_complete = ros::Time::now(); + if (optimization_complete > optimization_deadline) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); + } + } + } +} + +} // namespace fuse_optimizers diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp new file mode 100644 index 000000000..4dbe52a9c --- /dev/null +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp @@ -0,0 +1,51 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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. + */ +#include +#include +#include +#include + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "step_wise_fixed_lag_smoother_node"); + ros::NodeHandle private_node_handle("~"); + fuse_graphs::HashGraphParams hash_graph_params; + hash_graph_params.loadFromROS(private_node_handle); + fuse_optimizers::StepWiseFixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + optimizer.startOptimization(); + ros::spin(); + + return 0; +} From 24fbec4c0904528d50123c8b889641017d2a1cd8 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Tue, 28 Feb 2023 11:57:00 +0100 Subject: [PATCH 2/4] add example launch file/config for stepwise optimization --- ...sensor_stepwise_optimization_tutorial.yaml | 72 +++++++++++++++++++ ...nsor_stepwise_optimization_tutorial.launch | 15 ++++ 2 files changed, 87 insertions(+) create mode 100644 fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml create mode 100644 fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch diff --git a/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml b/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml new file mode 100644 index 000000000..7da7fdfbb --- /dev/null +++ b/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml @@ -0,0 +1,72 @@ +# Step-wise Fixed-lag smoother configuration +optimization_frequency: 20 +transaction_timeout: 0.01 +lag_duration: 0.5 + +optimization_steps: + - + sources: + - "initial_localization_sensor" + - "unicycle_motion_model" + - + sources: + - "odometry_sensor" + - "imu_sensor" + - + sources: + - "range_sensor" + +motion_models: + unicycle_motion_model: + type: fuse_models::Unicycle2D + +unicycle_motion_model: + # x y yaw vx vy vyaw ax ay + process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1] + +sensor_models: + initial_localization_sensor: + type: fuse_models::Unicycle2DIgnition + motion_models: [unicycle_motion_model] + ignition: true + odometry_sensor: + type: fuse_models::Odometry2D + motion_models: [unicycle_motion_model] + imu_sensor: + type: fuse_models::Imu2D + motion_models: [unicycle_motion_model] + range_sensor: + type: fuse_tutorials::RangeSensorModel + motion_models: [unicycle_motion_model] + +initial_localization_sensor: + publish_on_startup: false + +odometry_sensor: + topic: 'wheel_odom' + linear_velocity_dimensions: ['x', 'y'] + angular_velocity_dimensions: ['yaw'] + +imu_sensor: + topic: 'imu' + angular_velocity_dimensions: ['yaw'] + +range_sensor: + {} + +publishers: + filtered_publisher: + type: fuse_models::Odometry2DPublisher + beacon_publisher: + type: fuse_tutorials::BeaconPublisher + +filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'map' + publish_tf: false + +beacon_publisher: + map_frame_id: 'map' diff --git a/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch b/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch new file mode 100644 index 000000000..b566f2917 --- /dev/null +++ b/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + From 423a2d1681da61da57fcddc85eed788bc99f8233 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Wed, 8 Mar 2023 14:52:12 +0100 Subject: [PATCH 3/4] fix linter errors caused by step-wise fixed-lag smoother --- .../step_wise_fixed_lag_smoother.h | 2 -- .../step_wise_fixed_lag_smoother_params.h | 15 ++++++++------- .../src/step_wise_fixed_lag_smoother.cpp | 11 ++++------- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h index 8b5e05118..62bce5fe8 100644 --- a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h @@ -166,10 +166,8 @@ class StepWiseFixedLagSmoother : public FixedLagSmoother void startOptimization() override; protected: - // Read-only after construction ParameterType params_; //!< Configuration settings for this fixed-lag smoother - }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h index 0b8ced5df..76ff18de2 100644 --- a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h @@ -57,11 +57,11 @@ namespace fuse_optimizers struct StepWiseFixedLagSmootherParams : FixedLagSmootherParams { public: - /** * @brief The ordered list of steps for the optimization with each source inside * - * The optimization is performed sequentially where each step adds more constraints from different sources to the graph. + * The optimization is performed sequentially where each step adds + * more constraints from different sources to the graph. */ std::vector> optimization_steps; @@ -73,13 +73,13 @@ struct StepWiseFixedLagSmootherParams : FixedLagSmootherParams void loadFromROS(const ros::NodeHandle& nh) { FixedLagSmootherParams::loadFromROS(nh); - + // load optimization steps XmlRpc::XmlRpcValue optimization_steps_raw; fuse_core::getParamRequired(nh, "optimization_steps", optimization_steps_raw); ROS_ASSERT(optimization_steps_raw.getType() == XmlRpc::XmlRpcValue::TypeArray); - + for (int step_i = 0; step_i < optimization_steps_raw.size(); ++step_i) { ROS_ASSERT(optimization_steps_raw[step_i].getType() == XmlRpc::XmlRpcValue::TypeStruct); @@ -87,14 +87,15 @@ struct StepWiseFixedLagSmootherParams : FixedLagSmootherParams ROS_ASSERT(optimization_steps_raw[step_i]["sources"].getType() == XmlRpc::XmlRpcValue::TypeArray); std::vector optimization_step_sources; - + for (int source_i = 0; source_i < optimization_steps_raw[step_i]["sources"].size(); ++source_i) { ROS_ASSERT(optimization_steps_raw[step_i]["sources"][source_i].getType() == XmlRpc::XmlRpcValue::TypeString); - optimization_step_sources.push_back(static_cast(optimization_steps_raw[step_i]["sources"][source_i])); + optimization_step_sources.push_back( + static_cast(optimization_steps_raw[step_i]["sources"][source_i])); } - + optimization_steps.push_back(optimization_step_sources); } } diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp index d29c07254..b07bcfa7a 100644 --- a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp @@ -63,15 +63,12 @@ StepWiseFixedLagSmoother::StepWiseFixedLagSmoother( // add its own marginal constraints source to the optimization steps if (params_.optimization_steps.empty()) { - params_.optimization_steps.push_back({ros::this_node::getName()}); + params_.optimization_steps.push_back({ros::this_node::getName()}); // NOLINT } else { params_.optimization_steps[0].push_back(ros::this_node::getName()); } - - // Start the optimization thread - // optimization_thread_ = std::thread(&StepWiseFixedLagSmoother::optimizationLoop, this); } void StepWiseFixedLagSmoother::startOptimization() @@ -150,7 +147,7 @@ void StepWiseFixedLagSmoother::optimizationLoop() { ROS_DEBUG_STREAM("type: " << type); } - + fuse_core::Transaction::SharedPtr new_transaction_for_loop = new_transaction->clone(); // Keep only the sensor models that will be optimized in this loop @@ -168,7 +165,7 @@ void StepWiseFixedLagSmoother::optimizationLoop() { new_transaction_for_loop->removeConstraint(i); } - + // skip if there are no elements for optimization in this optimization step if (boost::empty(new_transaction_for_loop->addedConstraints()) || new_transaction_for_loop->empty()) { @@ -177,7 +174,7 @@ void StepWiseFixedLagSmoother::optimizationLoop() } ROS_DEBUG_STREAM("New transaction within optimization step: " << *new_transaction_for_loop); - + // Update the graph try { From ac9919025be0573ae993bde36dcac1242c956736 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Wed, 8 Mar 2023 15:46:30 +0100 Subject: [PATCH 4/4] add missing includes where the build was still working but the linter noticed them --- fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp index b07bcfa7a..ee5b5d0dd 100644 --- a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp @@ -47,7 +47,8 @@ #include #include #include - +#include +#include namespace fuse_optimizers {