From 6878097c823fe4a53b967a09c21a458f70c9c97a Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Fri, 21 Jul 2023 10:48:31 +0200 Subject: [PATCH 001/116] humble branch --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 3908121b1..7f4fd5d89 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # fuse -**NOTE**: The `rolling` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done! +**NOTE**: The `humble` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done! The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications include state estimation, localization, mapping, and calibration. From edb7d601880e9793d098c6dbc73335f003a0b4d8 Mon Sep 17 00:00:00 2001 From: Giacomo Date: Fri, 4 Aug 2023 16:46:18 +0000 Subject: [PATCH 002/116] make composable optimizer nodes (dev) --- fuse_optimizers/CMakeLists.txt | 30 +++++++++- .../fuse_optimizers/batch_optimizer.hpp | 9 +++ .../fuse_optimizers/fixed_lag_smoother.hpp | 9 +++ .../include/fuse_optimizers/optimizer.hpp | 26 +++++++++ fuse_optimizers/src/batch_optimizer.cpp | 7 +++ .../src/batch_optimizer_component.cpp | 5 ++ fuse_optimizers/src/fixed_lag_smoother.cpp | 43 ++++++++++++++ .../src/fixed_lag_smoother_component.cpp | 5 ++ fuse_optimizers/src/optimizer.cpp | 57 +++++++++++++++++++ 9 files changed, 190 insertions(+), 1 deletion(-) create mode 100644 fuse_optimizers/src/batch_optimizer_component.cpp create mode 100644 fuse_optimizers/src/fixed_lag_smoother_component.cpp diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 6f21b1ce8..febce1b56 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -62,6 +62,34 @@ target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) +## batch_optimizer component +add_library(batch_optimizer_component SHARED src/batch_optimizer_component.cpp) +target_include_directories(batch_optimizer_component PRIVATE include) +ament_target_dependencies( + batch_optimizer_component + SYSTEM + rclcpp_components +) +target_link_libraries(batch_optimizer_component ${PROJECT_NAME}) + +rclcpp_components_register_node( + batch_optimizer_component PLUGIN "fuse_optimizers::BatchOptimizer" EXECUTABLE + BatchOptimizerExecutable) + +## fixed_lag_smoother component +add_library(fixed_lag_smoother_component SHARED src/fixed_lag_smoother_component.cpp) +target_include_directories(fixed_lag_smoother_component PRIVATE include) +ament_target_dependencies( + fixed_lag_smoother_component + SYSTEM + rclcpp_components +) +target_link_libraries(fixed_lag_smoother_component ${PROJECT_NAME}) + +rclcpp_components_register_node( + fixed_lag_smoother_component PLUGIN "fuse_optimizers::FixedLagSmoother" EXECUTABLE + FixedLagSmootherExecutable) + ############# ## Testing ## ############# @@ -76,7 +104,7 @@ endif() ## Install ## ############# -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install(TARGETS ${PROJECT_NAME} batch_optimizer_component fixed_lag_smoother_component EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index 48e9dc254..370a5166a 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -106,6 +106,15 @@ class BatchOptimizer : public Optimizer FUSE_SMART_PTR_DEFINITIONS(BatchOptimizer) using ParameterType = BatchOptimizerParams; +/** + * @brief Component default constructor + * + * @param[in] options rclcpp node options + */ + BatchOptimizer( + const rclcpp::NodeOptions & options + ); + /** * @brief Constructor * diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index af60d1a37..46dc6cd4c 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -119,6 +119,15 @@ class FixedLagSmoother : public Optimizer FUSE_SMART_PTR_DEFINITIONS(FixedLagSmoother) using ParameterType = FixedLagSmootherParams; + /** + * @brief Component constructor + * + * @param[in] options rclcpp node options + */ + explicit FixedLagSmoother( + const rclcpp::NodeOptions & options + ); + /** * @brief Constructor * diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index ce6dc6729..b32280f22 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -98,6 +98,19 @@ class Optimizer public: FUSE_SMART_PTR_ALIASES_ONLY(Optimizer) + /** + * @brief Component Constructor + * + * @param[in] node_name Name for rclcpp Node + * @param[in] options Options for rclcpp Node + * @param[in] graph The graph used with the optimizer + */ + Optimizer( + const std::string& node_name, + const rclcpp::NodeOptions & options, + fuse_core::Graph::UniquePtr graph = nullptr + ); + /** * @brief Constructor * @@ -114,7 +127,20 @@ class Optimizer */ virtual ~Optimizer(); + /** + * @brief get_node_base_interface method needed for composition + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); + protected: + + /** + * @brief Shared ptr to optimizer node + */ + + // TODO node type should be generalized + std::shared_ptr node_; + // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type // rather annoying as it is not equivalent to Class::UniquePtr using MotionModelUniquePtr = class_loader::ClassLoader::UniquePtr; diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index a4b520706..c5bfd4f40 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -47,6 +47,13 @@ namespace fuse_optimizers { +BatchOptimizer::BatchOptimizer( + const rclcpp::NodeOptions & options +) +: BatchOptimizer::BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces() , nullptr) +{ +} + BatchOptimizer::BatchOptimizer( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph diff --git a/fuse_optimizers/src/batch_optimizer_component.cpp b/fuse_optimizers/src/batch_optimizer_component.cpp new file mode 100644 index 000000000..b37b5953e --- /dev/null +++ b/fuse_optimizers/src/batch_optimizer_component.cpp @@ -0,0 +1,5 @@ +#include "fuse_optimizers/batch_optimizer.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizer) \ No newline at end of file diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f80960862..fc5a307e3 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -78,6 +78,49 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { +FixedLagSmoother::FixedLagSmoother( + const rclcpp::NodeOptions & options +) +: fuse_optimizers::Optimizer("fixed_lag_smoother_node", options), + ignited_(false), + optimization_running_(true), + started_(false), + optimization_request_(false) +{ + params_.loadFromROS(interfaces_); + + // Test for auto-start + autostart(); + + // Start the optimization thread + optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); + + // Configure a timer to trigger optimizations + optimize_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + params_.optimization_period, + std::bind(&FixedLagSmoother::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group() + ); + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName( + interfaces_.get_node_base_interface()->get_name(), + params_.reset_service), + std::bind( + &FixedLagSmoother::resetServiceCallback, + this, + std::placeholders::_1, + std::placeholders::_2 + ), + rclcpp::ServicesQoS(), + interfaces_.get_node_base_interface()->get_default_callback_group() + ); +} + FixedLagSmoother::FixedLagSmoother( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph diff --git a/fuse_optimizers/src/fixed_lag_smoother_component.cpp b/fuse_optimizers/src/fixed_lag_smoother_component.cpp new file mode 100644 index 000000000..4f0ffb216 --- /dev/null +++ b/fuse_optimizers/src/fixed_lag_smoother_component.cpp @@ -0,0 +1,5 @@ +#include "fuse_optimizers/fixed_lag_smoother.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmoother) \ No newline at end of file diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 2a4b62647..1f5854013 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -52,6 +52,58 @@ namespace fuse_optimizers { +Optimizer::Optimizer( + const std::string& node_name, + const rclcpp::NodeOptions & options, + fuse_core::Graph::UniquePtr graph +) +: node_ (std::make_shared(node_name, options)), + interfaces_(fuse_core::node_interfaces::NodeInterfaces(*node_)), + clock_(interfaces_.get_node_clock_interface()->get_clock()), + logger_(interfaces_.get_node_logging_interface()->get_logger()), + graph_(std::move(graph)), + motion_model_loader_("fuse_core", "fuse_core::MotionModel"), + publisher_loader_("fuse_core", "fuse_core::Publisher"), + sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), + diagnostic_updater_( + interfaces_.get_node_base_interface(), + interfaces_.get_node_clock_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_timers_interface(), + interfaces_.get_node_topics_interface() + ), + callback_queue_(std::make_shared( + interfaces_.get_node_base_interface()->get_context())) +{ + RCLCPP_INFO(logger_, "Running optimizer constructor"); + if (!graph) { + fuse_graphs::HashGraphParams hash_graph_params; + hash_graph_params.loadFromROS(interfaces_); + graph_ = std::move(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + } + + // add a ros1 style callback queue so that transactions can be processed in the optimiser's + // executor + interfaces_.get_node_waitables_interface()->add_waitable( + callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); + + diagnostic_updater_.add( + interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); + diagnostic_updater_.setHardwareID("fuse"); + + // Wait for a valid time before loading any of the plugins + clock_->wait_until_started(); + + // Load all configured plugins + loadMotionModels(); + loadSensorModels(); + loadPublishers(); + + // Start all the plugins + startPlugins(); +} + Optimizer::Optimizer( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph @@ -516,4 +568,9 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Optimizer::get_node_base_interface() +{ + return interfaces_.get_node_base_interface(); +} + } // namespace fuse_optimizers From 504a1c676b38e2334196e48c06f81a7e7e966f70 Mon Sep 17 00:00:00 2001 From: Giacomo Date: Mon, 7 Aug 2023 14:59:29 +0000 Subject: [PATCH 003/116] batch and fixed lag optimizer as components --- .../fuse_optimizers/batch_optimizer.hpp | 6 +++--- fuse_optimizers/src/batch_optimizer.cpp | 19 ++++++++++++++++++- fuse_optimizers/src/fixed_lag_smoother.cpp | 5 +++++ fuse_optimizers/src/optimizer.cpp | 2 +- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index 370a5166a..fb0d89265 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -107,11 +107,11 @@ class BatchOptimizer : public Optimizer using ParameterType = BatchOptimizerParams; /** - * @brief Component default constructor + * @brief Component constructor * - * @param[in] options rclcpp node options + * @param[in] options rclcpp node options */ - BatchOptimizer( + explicit BatchOptimizer( const rclcpp::NodeOptions & options ); diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index c5bfd4f40..8d5e08414 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -50,8 +50,25 @@ namespace fuse_optimizers BatchOptimizer::BatchOptimizer( const rclcpp::NodeOptions & options ) -: BatchOptimizer::BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces() , nullptr) +: fuse_optimizers::Optimizer("batch_optimizer_node", options), + combined_transaction_(fuse_core::Transaction::make_shared()), + optimization_request_(false), + start_time_(rclcpp::Time::max()), + started_(false) { + params_.loadFromROS(interfaces_); + + // Configure a timer to trigger optimizations + optimize_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + params_.optimization_period, + std::bind(&BatchOptimizer::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group() + ); + + // Start the optimization thread + optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); } BatchOptimizer::BatchOptimizer( diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index fc5a307e3..51f0c5c2b 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -239,6 +239,7 @@ void FixedLagSmoother::optimizationLoop() while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) { + RCLCPP_DEBUG(logger_, "In the optimization loop"); // Wait for the next signal to start the next optimization cycle // NOTE(CH3): Uninitialized, but it's ok since it's meant to get overwritten. auto optimization_deadline = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -256,6 +257,7 @@ void FixedLagSmoother::optimizationLoop() } // Optimize { + RCLCPP_DEBUG(logger_, "optimizing"); std::lock_guard lock(optimization_mutex_); // Apply motion models auto new_transaction = fuse_core::Transaction::make_shared(); @@ -333,7 +335,9 @@ void FixedLagSmoother::optimizationLoop() << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); } } + RCLCPP_DEBUG(logger_, "Ending while loop"); } + RCLCPP_DEBUG(logger_, "optimization finished"); } void FixedLagSmoother::optimizerTimerCallback() @@ -543,6 +547,7 @@ void FixedLagSmoother::transactionCallback( fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it + RCLCPP_DEBUG(logger_, "transactionCallback: inside transactionCallback"); auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 1f5854013..e5fdc1af7 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -57,7 +57,7 @@ Optimizer::Optimizer( const rclcpp::NodeOptions & options, fuse_core::Graph::UniquePtr graph ) -: node_ (std::make_shared(node_name, options)), +: node_(std::make_shared(node_name, options)), interfaces_(fuse_core::node_interfaces::NodeInterfaces(*node_)), clock_(interfaces_.get_node_clock_interface()->get_clock()), logger_(interfaces_.get_node_logging_interface()->get_logger()), From 699b9d15623370e73065acf90f75ac987c3919f7 Mon Sep 17 00:00:00 2001 From: Giacomo Date: Wed, 23 Aug 2023 14:19:02 +0000 Subject: [PATCH 004/116] optimizer components refactor --- fuse_optimizers/CMakeLists.txt | 16 +++--- .../fuse_optimizers/batch_optimizer.hpp | 9 ---- .../batch_optimizer_component.hpp | 20 ++++++++ .../fuse_optimizers/fixed_lag_smoother.hpp | 9 ---- .../fixed_lag_smoother_component.hpp | 19 +++++++ .../include/fuse_optimizers/optimizer.hpp | 27 +--------- fuse_optimizers/src/batch_optimizer.cpp | 24 --------- .../src/batch_optimizer_component.cpp | 13 ++++- fuse_optimizers/src/batch_optimizer_node.cpp | 47 ------------------ fuse_optimizers/src/fixed_lag_smoother.cpp | 43 ---------------- .../src/fixed_lag_smoother_component.cpp | 14 +++++- .../src/fixed_lag_smoother_node.cpp | 49 ------------------- 12 files changed, 71 insertions(+), 219 deletions(-) create mode 100644 fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp create mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp delete mode 100644 fuse_optimizers/src/batch_optimizer_node.cpp delete mode 100644 fuse_optimizers/src/fixed_lag_smoother_node.cpp diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index febce1b56..2b26f549c 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -55,12 +55,12 @@ target_link_libraries(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} diagnostic_updater) ## batch_optimizer node -add_executable(batch_optimizer_node src/batch_optimizer_node.cpp) -target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) +# add_executable(batch_optimizer_node src/batch_optimizer_node.cpp) +# target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) ## fixed_lag_smoother node -add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) -target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) +# add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) +# target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) ## batch_optimizer component add_library(batch_optimizer_component SHARED src/batch_optimizer_component.cpp) @@ -73,8 +73,8 @@ ament_target_dependencies( target_link_libraries(batch_optimizer_component ${PROJECT_NAME}) rclcpp_components_register_node( - batch_optimizer_component PLUGIN "fuse_optimizers::BatchOptimizer" EXECUTABLE - BatchOptimizerExecutable) + batch_optimizer_component PLUGIN "fuse_optimizers::BatchOptimizerComponent" EXECUTABLE + batch_optimizer_node) ## fixed_lag_smoother component add_library(fixed_lag_smoother_component SHARED src/fixed_lag_smoother_component.cpp) @@ -87,8 +87,8 @@ ament_target_dependencies( target_link_libraries(fixed_lag_smoother_component ${PROJECT_NAME}) rclcpp_components_register_node( - fixed_lag_smoother_component PLUGIN "fuse_optimizers::FixedLagSmoother" EXECUTABLE - FixedLagSmootherExecutable) + fixed_lag_smoother_component PLUGIN "fuse_optimizers::FixedLagSmootherComponent" EXECUTABLE + fixed_lag_smoother_node) ############# ## Testing ## diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index fb0d89265..48e9dc254 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -106,15 +106,6 @@ class BatchOptimizer : public Optimizer FUSE_SMART_PTR_DEFINITIONS(BatchOptimizer) using ParameterType = BatchOptimizerParams; -/** - * @brief Component constructor - * - * @param[in] options rclcpp node options - */ - explicit BatchOptimizer( - const rclcpp::NodeOptions & options - ); - /** * @brief Constructor * diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp new file mode 100644 index 000000000..37306090c --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp @@ -0,0 +1,20 @@ +#ifndef FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ +#define FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "fuse_optimizers/batch_optimizer.hpp" + +namespace fuse_optimizers +{ + class BatchOptimizerComponent : public rclcpp::Node + { + public: + explicit BatchOptimizerComponent(const rclcpp::NodeOptions& options); + + + private: + std::shared_ptr batch_optimizer_; + }; +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index 46dc6cd4c..af60d1a37 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -119,15 +119,6 @@ class FixedLagSmoother : public Optimizer FUSE_SMART_PTR_DEFINITIONS(FixedLagSmoother) using ParameterType = FixedLagSmootherParams; - /** - * @brief Component constructor - * - * @param[in] options rclcpp node options - */ - explicit FixedLagSmoother( - const rclcpp::NodeOptions & options - ); - /** * @brief Constructor * diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp new file mode 100644 index 000000000..a27aab423 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp @@ -0,0 +1,19 @@ +#ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ +#define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ + +#include +#include + +namespace fuse_optimizers +{ + class FixedLagSmootherComponent : public rclcpp::Node + { + public: + explicit FixedLagSmootherComponent(const rclcpp::NodeOptions& options); + + private: + std::shared_ptr fixed_lag_smoother_; + }; +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index b32280f22..0cc1c956a 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -98,20 +98,7 @@ class Optimizer public: FUSE_SMART_PTR_ALIASES_ONLY(Optimizer) - /** - * @brief Component Constructor - * - * @param[in] node_name Name for rclcpp Node - * @param[in] options Options for rclcpp Node - * @param[in] graph The graph used with the optimizer - */ - Optimizer( - const std::string& node_name, - const rclcpp::NodeOptions & options, - fuse_core::Graph::UniquePtr graph = nullptr - ); - - /** + /** * @brief Constructor * * @param[in] interfaces The node interfaces for the node driving the optimizer @@ -127,20 +114,8 @@ class Optimizer */ virtual ~Optimizer(); - /** - * @brief get_node_base_interface method needed for composition - */ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); - protected: - /** - * @brief Shared ptr to optimizer node - */ - - // TODO node type should be generalized - std::shared_ptr node_; - // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type // rather annoying as it is not equivalent to Class::UniquePtr using MotionModelUniquePtr = class_loader::ClassLoader::UniquePtr; diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index 8d5e08414..a4b520706 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -47,30 +47,6 @@ namespace fuse_optimizers { -BatchOptimizer::BatchOptimizer( - const rclcpp::NodeOptions & options -) -: fuse_optimizers::Optimizer("batch_optimizer_node", options), - combined_transaction_(fuse_core::Transaction::make_shared()), - optimization_request_(false), - start_time_(rclcpp::Time::max()), - started_(false) -{ - params_.loadFromROS(interfaces_); - - // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&BatchOptimizer::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); - - // Start the optimization thread - optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); -} - BatchOptimizer::BatchOptimizer( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph diff --git a/fuse_optimizers/src/batch_optimizer_component.cpp b/fuse_optimizers/src/batch_optimizer_component.cpp index b37b5953e..113792cfa 100644 --- a/fuse_optimizers/src/batch_optimizer_component.cpp +++ b/fuse_optimizers/src/batch_optimizer_component.cpp @@ -1,5 +1,14 @@ -#include "fuse_optimizers/batch_optimizer.hpp" +#include "fuse_optimizers/batch_optimizer_component.hpp" +namespace fuse_optimizers +{ + BatchOptimizerComponent::BatchOptimizerComponent(const rclcpp::NodeOptions& options) + : Node("batch_optimizer", options), + batch_optimizer_(std::make_shared(*this)) + { + RCLCPP_INFO(get_logger(), "BatchOptimizerComponent constructed"); + } +} // namespace fuse_optimizers #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizer) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizerComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp deleted file mode 100644 index e7d00b85f..000000000 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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. - */ - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("batch_optimizer_node"); - auto optimizer = std::make_shared(*node); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 51f0c5c2b..0543cde55 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -78,49 +78,6 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { -FixedLagSmoother::FixedLagSmoother( - const rclcpp::NodeOptions & options -) -: fuse_optimizers::Optimizer("fixed_lag_smoother_node", options), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) -{ - params_.loadFromROS(interfaces_); - - // Test for auto-start - autostart(); - - // Start the optimization thread - optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); - - // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&FixedLagSmoother::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); - // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.reset_service), - std::bind( - &FixedLagSmoother::resetServiceCallback, - this, - std::placeholders::_1, - std::placeholders::_2 - ), - rclcpp::ServicesQoS(), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); -} - FixedLagSmoother::FixedLagSmoother( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph diff --git a/fuse_optimizers/src/fixed_lag_smoother_component.cpp b/fuse_optimizers/src/fixed_lag_smoother_component.cpp index 4f0ffb216..66f650a07 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_component.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_component.cpp @@ -1,5 +1,15 @@ -#include "fuse_optimizers/fixed_lag_smoother.hpp" +#include "fuse_optimizers/fixed_lag_smoother_component.hpp" + +namespace fuse_optimizers +{ + FixedLagSmootherComponent::FixedLagSmootherComponent(const rclcpp::NodeOptions& options) + : Node("fixed_lag_smoother", options), + fixed_lag_smoother_(std::make_shared(*this)) + { + RCLCPP_INFO(get_logger(), "FixedLagSmootherComponent constructed"); + } +} // namespace fuse_optimizers #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmoother) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmootherComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp deleted file mode 100644 index 7be160efa..000000000 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, 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. - */ - -#include -#include -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("fixed_lag_smoother_node"); - auto optimizer = std::make_shared(*node); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} From ce960adb85b9c65ba854c1ca16d876ef3fb65dd1 Mon Sep 17 00:00:00 2001 From: Giacomo Date: Wed, 23 Aug 2023 14:21:06 +0000 Subject: [PATCH 005/116] undo optimizer.cpp changes --- fuse_optimizers/src/optimizer.cpp | 57 ------------------------------- 1 file changed, 57 deletions(-) diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index e5fdc1af7..2a4b62647 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -52,58 +52,6 @@ namespace fuse_optimizers { -Optimizer::Optimizer( - const std::string& node_name, - const rclcpp::NodeOptions & options, - fuse_core::Graph::UniquePtr graph -) -: node_(std::make_shared(node_name, options)), - interfaces_(fuse_core::node_interfaces::NodeInterfaces(*node_)), - clock_(interfaces_.get_node_clock_interface()->get_clock()), - logger_(interfaces_.get_node_logging_interface()->get_logger()), - graph_(std::move(graph)), - motion_model_loader_("fuse_core", "fuse_core::MotionModel"), - publisher_loader_("fuse_core", "fuse_core::Publisher"), - sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_( - interfaces_.get_node_base_interface(), - interfaces_.get_node_clock_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_timers_interface(), - interfaces_.get_node_topics_interface() - ), - callback_queue_(std::make_shared( - interfaces_.get_node_base_interface()->get_context())) -{ - RCLCPP_INFO(logger_, "Running optimizer constructor"); - if (!graph) { - fuse_graphs::HashGraphParams hash_graph_params; - hash_graph_params.loadFromROS(interfaces_); - graph_ = std::move(fuse_graphs::HashGraph::make_unique(hash_graph_params)); - } - - // add a ros1 style callback queue so that transactions can be processed in the optimiser's - // executor - interfaces_.get_node_waitables_interface()->add_waitable( - callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); - - diagnostic_updater_.add( - interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); - diagnostic_updater_.setHardwareID("fuse"); - - // Wait for a valid time before loading any of the plugins - clock_->wait_until_started(); - - // Load all configured plugins - loadMotionModels(); - loadSensorModels(); - loadPublishers(); - - // Start all the plugins - startPlugins(); -} - Optimizer::Optimizer( fuse_core::node_interfaces::NodeInterfaces interfaces, fuse_core::Graph::UniquePtr graph @@ -568,9 +516,4 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Optimizer::get_node_base_interface() -{ - return interfaces_.get_node_base_interface(); -} - } // namespace fuse_optimizers From dce95d2fcc71f98d5f832caaf7f4f6c6ad8506ac Mon Sep 17 00:00:00 2001 From: Giacomo Date: Mon, 25 Sep 2023 09:22:59 +0000 Subject: [PATCH 006/116] set precision 1e-4 to validatePartialMeasurement --- fuse_models/include/fuse_models/common/sensor_proc.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 40bb4bc14..314029951 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -357,7 +357,7 @@ inline bool processAbsolutePoseWithCovariance( if (validate) { try { - validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-4); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -912,7 +912,7 @@ inline bool processDifferentialPoseWithTwistCovariance( try { validatePartialMeasurement( pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-6); + 1e-4); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -1044,7 +1044,7 @@ inline bool processTwistWithCovariance( if (validate) { try { - validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial); + validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-4); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -1084,7 +1084,7 @@ inline bool processTwistWithCovariance( if (validate) { try { - validatePartialMeasurement(angular_vel_vector, angular_vel_covariance); + validatePartialMeasurement(angular_vel_vector, angular_vel_covariance, 1e-4); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, @@ -1197,7 +1197,7 @@ inline bool processAccelWithCovariance( if (validate) { try { - validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, From f33bc5df3601481007e2540c0e8fb0b488ad96c0 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 5 Oct 2023 09:27:14 +0000 Subject: [PATCH 007/116] new branch iron_unicycle3d --- fuse_core/include/fuse_core/eigen.hpp | 5 + fuse_models/CMakeLists.txt | 3 + fuse_models/fuse_plugins.xml | 13 + .../fuse_models/common/sensor_proc.hpp | 52 ++ .../include/fuse_models/unicycle_3d.hpp | 246 +++++++ .../fuse_models/unicycle_3d_ignition.hpp | 220 ++++++ .../fuse_models/unicycle_3d_predict.hpp | 637 +++++++++++++++++ .../unicycle_3d_state_cost_function.hpp | 301 ++++++++ .../unicycle_3d_state_cost_functor.hpp | 204 ++++++ ...unicycle_3d_state_kinematic_constraint.hpp | 188 +++++ fuse_models/src/unicycle_2d.cpp | 7 + fuse_models/src/unicycle_3d.cpp | 651 ++++++++++++++++++ fuse_models/src/unicycle_3d_ignition.cpp | 408 +++++++++++ ...unicycle_3d_state_kinematic_constraint.cpp | 118 ++++ .../test_unicycle_3d_state_cost_function.cpp | 144 ++++ 15 files changed, 3197 insertions(+) create mode 100644 fuse_models/include/fuse_models/unicycle_3d.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_ignition.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_predict.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp create mode 100644 fuse_models/src/unicycle_3d.cpp create mode 100644 fuse_models/src/unicycle_3d_ignition.cpp create mode 100644 fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp create mode 100644 fuse_models/test/test_unicycle_3d_state_cost_function.cpp diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 6d9ced923..905ecb081 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -54,6 +54,7 @@ using Vector6d = Eigen::Matrix; using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; +using Vector15d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; @@ -65,6 +66,10 @@ using Matrix6d = Eigen::Matrix; using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; +using Matrix15d = Eigen::Matrix; +using Matrix16d = Eigen::Matrix; + +using Quaternion = Eigen::Quaternion; template using Matrix = Eigen::Matrix; diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index fe3bc9452..dbe3c8574 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -51,6 +51,9 @@ add_library(${PROJECT_NAME} src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp + src/unicycle_3d.cpp + # src/unicycle_3d_ignition.cpp + src/unicycle_3d_state_kinematic_constraint.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index f6c860934..809c4c429 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -12,6 +12,19 @@ + + + A class that represents a kinematic constraint between 3D states at two different times. + + + + + + A fuse_models 3D kinematic model that generates kinematic constraints between provided time stamps, and adds + those constraints to the fuse graph. + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 2D diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 314029951..4a2f317ed 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -1266,6 +1266,58 @@ inline void scaleProcessNoiseCovariance( velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } +/** + * @brief Scales the process noise covariance pose by the norm of the velocity + * + * @param[in, out] process_noise_covariance - The process noise covariance to scale. Only the pose + * components (x, y, z, roll, pitch, yaw) are scaled, and they are assumed to be in the + * top left 6x6 corner + * @param[in] velocity_linear - The linear velocity + * @param[in] velocity_angular - The angular velocity + * @param[in] velocity_linear_norm_min - The minimum linear velocity norm + * @param[in] velocity_angular_norm_min - The minimum angular velocity norm + */ +inline void scaleProcessNoiseCovariance( + fuse_core::Matrix15d & process_noise_covariance, + const fuse_core::Vector3d & velocity_linear, + const fuse_core::Vector3d & velocity_angular, + const double velocity_linear_norm_min, + const double velocity_angular_norm_min) +{ + fuse_core::Matrix6d velocity; + velocity.setIdentity(); + velocity.topLeftCorner<3, 3>().diagonal() *= + std::max( + velocity_linear_norm_min, + velocity_linear.norm()); + velocity.bottomRightCorner<3, 3>().diagonal() *= + std::max( + velocity_angular_norm_min, + velocity_angular.norm()); + process_noise_covariance.topLeftCorner<6, 6>() = + velocity * process_noise_covariance.topLeftCorner<6, 6>() * velocity.transpose(); +} + +inline void scaleProcessNoiseCovariance( + fuse_core::Matrix16d & process_noise_covariance, + const fuse_core::Vector3d & velocity_linear, + const fuse_core::Vector3d & velocity_angular, + const double velocity_linear_norm_min, + const double velocity_angular_norm_min) +{ + fuse_core::Matrix3d velocity; + velocity.setIdentity(); + velocity.topLeftCorner<3, 3>().diagonal() *= + std::max( + velocity_linear_norm_min, + velocity_linear.norm()); + // velocity.bottomRightCorner<3, 3>().diagonal() *= + // std::max( + // velocity_angular_norm_min, + // velocity_angular.norm()); + process_noise_covariance.topLeftCorner<3, 3>() = + velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); +} } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/unicycle_3d.hpp new file mode 100644 index 000000000..050aacb13 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d.hpp @@ -0,0 +1,246 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_models +{ + +/** + * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided + * time stamps, and adds those constraints to the fuse graph. + * + * This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model + * in the robot_localization state estimation nodes. + * + * Parameters: + * - ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - ~device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds + * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal + * values for the process noise covariance matrix. + * Variable order is (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc). + */ +class Unicycle3D : public fuse_core::AsyncMotionModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Unicycle3D) + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Unicycle3D(); + + /** + * @brief Destructor + */ + ~Unicycle3D() = default; + + /** + * @brief Shadowing extension to the AsyncMotionModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; + + void print(std::ostream & stream = std::cout) const; + +protected: + /** + * @brief Structure used to maintain a history of "good" pose estimates + */ + struct StateHistoryElement + { + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable + + fuse_core::Vector3d position; //!< Map-frame position + // fuse_core::Vector3d orientation; //!< Map-frame orientation (roll, pitch, yaw) + fuse_core::Quaternion orientation; //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear; //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular; //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear; //!< Body-frame linear (angular not needed) accelerations + + void print(std::ostream & stream = std::cout) const; + + /** + * @brief Validate the state components: pose, linear velocities, angular velocities and linear + * accelerations. + * + * This validates the state components are finite. It throws an exception if any validation + * check fails. + */ + void validate() const; + }; + using StateHistory = std::map; + + /** + * @brief Augment a transaction structure such that the provided timestamps are connected by + * motion model constraints. + * @param[in] stamps The set of timestamps that should be connected by motion model + * constraints + * @param[out] transaction The transaction object that should be augmented with motion model + * constraints + * @return True if the motion models were generated successfully, false otherwise + */ + bool applyCallback(fuse_core::Transaction & transaction) override; + + /** + * @brief Generate a single motion model segment between the specified timestamps. + * + * This function is used by the timestamp manager to generate just the new motion model segments + * required to fulfill a query. + * + * @param[in] beginning_stamp The beginning timestamp of the motion model constraints to be + * generated. \p beginning_stamp is guaranteed to be less than \p + * ending_stamp. + * @param[in] ending_stamp The ending timestamp of the motion model constraints to be + * generated. \p ending_stamp is guaranteed to be greater than \p + * beginning_stamp. + * @param[out] constraints One or more motion model constraints between the requested + * timestamps. + * @param[out] variables One or more variables at both the \p beginning_stamp and \p + * ending_stamp. The variables should include initial values for the + * optimizer. + */ + void generateMotionModel( + const rclcpp::Time & beginning_stamp, + const rclcpp::Time & ending_stamp, + std::vector & constraints, + std::vector & variables); + + /** + * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received + * from the optimizer + * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed + * whenever needed. + */ + void onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required initialization for the kinematic model + */ + void onInit() override; + + /** + * @brief Reset the internal state history before starting + */ + void onStart() override; + + /** + * @brief Update all of the estimated states in the state history container using the optimized + * values from the graph + * @param[in] graph The graph object containing updated variable values + * @param[in] state_history The state history object to be updated + * @param[in] buffer_length States older than this in the history will be pruned + */ + static void updateStateHistoryEstimates( + const fuse_core::Graph & graph, + StateHistory & state_history, + const rclcpp::Duration & buffer_length); + + /** + * @brief Validate the motion model state #1, state #2 and process noise covariance + * + * This validates the motion model states and process noise covariance are valid. It throws an + * exception if any validation check fails. + * + * @param[in] state1 The first/oldest state + * @param[in] state2 The second/newest state + * @param[in] process_noise_covariance The process noise covariance, after it is scaled and + * multiplied by dt + */ + static void validateMotionModel( + const StateHistoryElement & state1, const StateHistoryElement & state2, + const fuse_core::Matrix16d & process_noise_covariance); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + rclcpp::Duration buffer_length_; //!< The length of the state history + fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created + //!< motion model segments + fuse_core::Matrix16d process_noise_covariance_; //!< Process noise covariance matrix + bool scale_process_noise_{false}; //!< Whether to scale the process noise + //!< covariance pose by the norm of the current + //!< state twist + double velocity_linear_norm_min_{1e-3}; //!< The minimum linear velocity norm allowed when + //!< scaling the process noise covariance + double velocity_angular_norm_min_{1e-3}; //!< The minimum twist norm allowed when + //!< scaling the process noise covariance + bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates +}; + +std::ostream & operator<<(std::ostream & stream, const Unicycle3D & unicycle_2d); + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp new file mode 100644 index 000000000..81d21b07d --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp @@ -0,0 +1,220 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief A fuse_models ignition sensor designed to be used in conjunction with the unicycle 3D + * motion model. + * + * This class publishes a transaction that contains a prior on each state subvariable used in the + * unicycle 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, and y_acc). When the sensor is first loaded, it publishes a single transaction + * with the configured initial state and covariance. + * Additionally, whenever a pose is received, either on the set_pose service or the topic, this + * ignition sensor resets the optimizer then publishes a new transaction with a prior at the + * specified pose. Priors on (x_vel, y_vel, yaw_vel, x_acc, and y_acc) continue to use the values + * configured on the parameter server. + * + * Parameters: + * - ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - ~device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - ~initial_sigma (vector of doubles) A 15-dimensional vector containing the standard deviations + * for the initial state values. The covariance matrix is + * created placing the squared standard deviations along the + * diagonal of an 15x15 matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). + * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for + * the state. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). + * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages + * - ~reset_service (string, default: "~/reset") The name of the reset service to call before + * sending a transaction + * - ~set_pose_deprecated_service (string, default: "set_pose_deprecated") The name of the + * set_pose_deprecated + * service + * - ~set_pose_service (string, default: "set_pose") The name of the set_pose service to advertise + * - ~topic (string, default: "set_pose") The topic name for received PoseWithCovarianceStamped + * messages + */ +class Unicycle3DIgnition : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Unicycle3DIgnition) + using ParameterType = parameters::Unicycle3DIgnitionParams; + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Unicycle3DIgnition(); + + /** + * @brief Destructor + */ + ~Unicycle3DIgnition() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + * + * As a very special case, we are overriding the start() method instead of providing an onStart() + * implementation. This is because the Unicycle3DIgnition sensor calls reset() on the optimizer, + * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of + * start() and stop(), the system would hang inside of one callback function while waiting for + * another callback to complete. + */ + void start() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + * + * As a very special case, we are overriding the stop() method instead of providing an onStop() + * implementation. This is because the Unicycle3DIgnition sensor calls reset() on the optimizer, + * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of + * start() and stop(), the system would hang inside of one callback function while waiting for + * another callback to complete. + */ + void stop() override; + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr, + const fuse_msgs::srv::SetPose::Request::SharedPtr req); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseDeprecatedServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + +protected: + /** + * @brief Perform any required initialization for the kinematic ignition sensor + */ + void onInit() override; + + /** + * @brief Process a received pose from one of the ROS comm channels + * + * This method validates the input pose, resets the optimizer, then constructs and sends the + * initial state constraints (by calling sendPrior()). + * + * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) + */ + void process( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + std::function post_process = nullptr); + + /** + * @brief Create and send a prior transaction based on the supplied pose + * + * The unicycle 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the + * parameter server. + * + * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) + */ + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Graph, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; //!< Object containing all of the configuration parameters + + //!< Service client used to call the "reset" service on the optimizer + rclcpp::Client::SharedPtr reset_client_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_pose_deprecated_service_; + + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp new file mode 100644 index 000000000..5c968f4ff --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -0,0 +1,637 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ + +#include +#include +#include + +#include +#include + +namespace fuse_models +{ +/** + * @brief Given a state and time delta, predicts a new state + computes the Jacobians + * @param[in] position - First position + * @param[in] orientation - First orientation + * @param[in] vel_linear - First linear velocity + * @param[in] vel_angular - First angular velocity + * @param[in] acc_linear - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position_out - Second position + * @param[out] orientation_out - Second orientation + * @param[out] vel_linear_out - Second linear velocity + * @param[out] vel_angular_out - Second angular velocity + * @param[out] acc_linear_out - Second linear acceleration + * @param[out] state_tf_jacobian - Jacobian of the motion model + */ +template +inline void predict( + const Eigen::Matrix & position, + const Eigen::Quaternion & orientation, + const Eigen::Matrix & vel_linear, + const Eigen::Matrix & vel_angular, + const Eigen::Matrix & acc_linear, + const T dt, + Eigen::Matrix & position_out, + Eigen::Quaternion & orientation_out, + Eigen::Matrix & vel_linear_out, + Eigen::Matrix & vel_angular_out, + Eigen::Matrix & acc_linear_out, + Eigen::Matrix* state_tf_jacobian = nullptr) +{ + // Convert quaternion to eigen + T roll = fuse_core::getRoll(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + T pitch = fuse_core::getPitch(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + T yaw = fuse_core::getYaw(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + // 3D material point projection model which matches the one used by r_l. + T sr = ceres::sin(roll); + T cr = ceres::cos(roll); + T sp = ceres::sin(pitch); + T cp = ceres::cos(pitch); + T sy = ceres::sin(yaw); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + T cy = ceres::cos(yaw); + T cpi = 1.0 / cp; + T tp = ceres::tan(pitch); + + Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; + Eigen::Matrix state_tf; + + tf_pos(0, 0) = (cy * cp) * dt; + tf_pos(0, 1) = (cy * sp * sr - sy * cr) * dt; + tf_pos(0, 2) = (cy * sp * cr + sy * sr) * dt; + tf_pos(1, 0) = (sy * cp) * dt; + tf_pos(1, 1) = (sy * sp * sr + cy * cr) * dt; + tf_pos(1, 2) = (sy * sp * cr - cy * sr) * dt; + tf_pos(2, 0) = (-sp) * dt; + tf_pos(2, 1) = (cp * sr) * dt; + tf_pos(2, 2) = (cp * cr) * dt; + + tf_rot(0, 0) = dt; + tf_rot(0, 1) = sr * sp * cpi * dt; + tf_rot(0, 2) = cr * sp * cpi * dt; + tf_rot(1, 0) = T(0); + tf_rot(1, 1) = cr * dt; + tf_rot(1, 2) = -sr * dt; + tf_rot(2, 0) = T(0); + tf_rot(2, 1) = sr * cpi * dt; + tf_rot(2, 2) = cr * cpi * dt; + + // Compute the transfer function matrix + state_tf.setZero(); + // position + // state_tf.block(0, 0) = Eigen::Matrix::Identity(); + // state_tf.block<3, 3>(0, 6) = tf_pos; + // state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; + // // orientation + // state_tf.block<3, 3>(3, 3) = Eigen::Matrix::Identity(); + // state_tf.block<3, 3>(3, 9) = tf_rot; + // // linear velocity + // state_tf.block<3, 3>(6, 6) = Eigen::Matrix::Identity(); + // state_tf.block<3, 3>(6, 12) = dt * Eigen::Matrix::Identity(); + // // angular velocity + // state_tf.block<3, 3>(9, 9) = Eigen::Matrix::Identity(); + // // linear acceleration + // state_tf.block<3, 3>(12, 12) = Eigen::Matrix::Identity(); + + if (state_tf_jacobian) + { + // TODO: compute the jacobian of the motion model transfer function + T x_coeff = T(0.0); + T y_coeff = T(0.0); + T z_coeff = T(0.0); + T one_half_at_squared = 0.5 * dt * dt; + + y_coeff = cy * sp * cr + sy * sr; + z_coeff = -cy * sp * sr + sy * cr; + T dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + T dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; + + x_coeff = -cy * sp; + y_coeff = cy * cp * sr; + z_coeff = cy * cp * cr; + T dFx_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + T dFR_dP = + (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; + + x_coeff = -sy * cp; + y_coeff = -sy * sp * sr - cy * cr; + z_coeff = -sy * sp * cr + cy * sr; + T dFx_dY = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + y_coeff = sy * sp * cr - cy * sr; + z_coeff = -sy * sp * sr - cy * cr; + T dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + T dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; + + x_coeff = -sy * sp; + y_coeff = sy * cp * sr; + z_coeff = sy * cp * cr; + T dFy_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + x_coeff = cy * cp; + y_coeff = cy * sp * sr - sy * cr; + z_coeff = cy * sp * cr + sy * sr; + T dFy_dY = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + y_coeff = cp * cr; + z_coeff = -cp * sr; + T dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + T dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; + + x_coeff = -cp; + y_coeff = -sp * sr; + z_coeff = -sp * cr; + T dFz_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + T dFY_dP = + (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; + + *state_tf_jacobian = state_tf; + (*state_tf_jacobian)(0, 3) = dFx_dR; + (*state_tf_jacobian)(0, 4) = dFx_dP; + (*state_tf_jacobian)(0, 5) = dFx_dY; + (*state_tf_jacobian)(1, 3) = dFy_dR; + (*state_tf_jacobian)(1, 4) = dFy_dP; + (*state_tf_jacobian)(1, 5) = dFy_dY; + (*state_tf_jacobian)(2, 3) = dFz_dR; + (*state_tf_jacobian)(2, 4) = dFz_dP; + (*state_tf_jacobian)(3, 3) = dFR_dR; + (*state_tf_jacobian)(3, 4) = dFR_dP; + (*state_tf_jacobian)(4, 3) = dFP_dR; + (*state_tf_jacobian)(5, 3) = dFY_dR; + (*state_tf_jacobian)(5, 4) = dFY_dP; + } + + // Predict the new state + Eigen::Matrix state; + state.head(3) = position; + state.segment(3,3) = Eigen::Matrix(roll, pitch, yaw); + state.segment(6,3) = vel_linear; + state.segment(9,3) = vel_angular; + state.tail(3) = acc_linear; + + state = state_tf * state; + + // Convert back + position_out = state.head(3); + fuse_core::wrapAngle2D(state(5)); + fuse_core::wrapAngle2D(state(4)); + fuse_core::wrapAngle2D(state(3)); + Eigen::Quaterniond q = + Eigen::AngleAxis(state(5), Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(state(4), Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(state(3), Eigen::Matrix::UnitX()); + orientation_out = q; + vel_linear_out = state.segment(6,3); + vel_angular_out = state.segment(9,3); + acc_linear_out = state.tail(3); +} + +/** + * @brief Given a state and time delta, predicts a new state + computes the Jacobians + * @param[in] position - First position + * @param[in] orientation - First orientation + * @param[in] vel_linear - First linear velocity + * @param[in] vel_angular - First angular velocity + * @param[in] acc_linear - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position_out - Second position + * @param[out] orientation_out - Second orientation + * @param[out] vel_linear_out - Second linear velocity + * @param[out] vel_angular_out - Second angular velocity + * @param[out] acc_linear_out - Second linear acceleration + * @param[out] state_tf_jacobian - Jacobian of the motion model + */ +inline void predict( + const fuse_core::Vector3d & position, + // const fuse_core::Vector3d & orientation, + const fuse_core::Quaternion & orientation, + const fuse_core::Vector3d & vel_linear, + const fuse_core::Vector3d & vel_angular, + const fuse_core::Vector3d & acc_linear, + const double dt, + fuse_core::Vector3d & position_out, + // fuse_core::Vector3d & orientation_out, + fuse_core::Quaternion & orientation_out, + fuse_core::Vector3d & vel_linear_out, + fuse_core::Vector3d & vel_angular_out, + fuse_core::Vector3d & acc_linear_out, + fuse_core::Matrix15d* state_tf_jacobian = nullptr) +{ + // Convert quaternion to eigen + double roll = fuse_core::getRoll(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + double pitch = fuse_core::getPitch(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + double yaw = fuse_core::getYaw(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + // 3D material point projection model which matches the one used by r_l. + double sr = ceres::sin(roll); + double cr = ceres::cos(roll); + double sp = ceres::sin(pitch); + double cp = ceres::cos(pitch); + double sy = ceres::sin(yaw); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + double cy = ceres::cos(yaw); + double cpi = 1.0 / cp; + double tp = ceres::tan(pitch); + + fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; + fuse_core::Matrix15d state_tf; + + tf_pos(0, 0) = (cy * cp) * dt; + tf_pos(0, 1) = (cy * sp * sr - sy * cr) * dt; + tf_pos(0, 2) = (cy * sp * cr + sy * sr) * dt; + tf_pos(1, 0) = (sy * cp) * dt; + tf_pos(1, 1) = (sy * sp * sr + cy * cr) * dt; + tf_pos(1, 2) = (sy * sp * cr - cy * sr) * dt; + tf_pos(2, 0) = (-sp) * dt; + tf_pos(2, 1) = (cp * sr) * dt; + tf_pos(2, 2) = (cp * cr) * dt; + + tf_rot(0, 0) = dt; + tf_rot(0, 1) = sr * sp * cpi * dt; + tf_rot(0, 2) = cr * sp * cpi * dt; + tf_rot(1, 0) = 0; + tf_rot(1, 1) = cr * dt; + tf_rot(1, 2) = -sr * dt; + tf_rot(2, 0) = 0; + tf_rot(2, 1) = sr * cpi * dt; + tf_rot(2, 2) = cr * cpi * dt; + + // Compute the transfer function matrix + state_tf.setZero(); + // position + state_tf.block<3, 3>(0, 0) = fuse_core::Matrix3d::Identity(); + state_tf.block<3, 3>(0, 6) = tf_pos; + state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; + // orientation + state_tf.block<3, 3>(3, 3) = fuse_core::Matrix3d::Identity(); + state_tf.block<3, 3>(3, 9) = tf_rot; + // linear velocity + state_tf.block<3, 3>(6, 6) = fuse_core::Matrix3d::Identity(); + state_tf.block<3, 3>(6, 12) = dt * fuse_core::Matrix3d::Identity(); + // angular velocity + state_tf.block<3, 3>(9, 9) = fuse_core::Matrix3d::Identity(); + // linear acceleration + state_tf.block<3, 3>(12, 12) = fuse_core::Matrix3d::Identity(); + + if (state_tf_jacobian) + { + // TODO: compute the jacobian of the motion model transfer function + double x_coeff = 0.0; + double y_coeff = 0.0; + double z_coeff = 0.0; + double one_half_at_squared = 0.5 * dt * dt; + + y_coeff = cy * sp * cr + sy * sr; + z_coeff = -cy * sp * sr + sy * cr; + double dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + double dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; + + x_coeff = -cy * sp; + y_coeff = cy * cp * sr; + z_coeff = cy * cp * cr; + double dFx_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + double dFR_dP = + (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; + + x_coeff = -sy * cp; + y_coeff = -sy * sp * sr - cy * cr; + z_coeff = -sy * sp * cr + cy * sr; + double dFx_dY = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + y_coeff = sy * sp * cr - cy * sr; + z_coeff = -sy * sp * sr - cy * cr; + double dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + double dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; + + x_coeff = -sy * sp; + y_coeff = sy * cp * sr; + z_coeff = sy * cp * cr; + double dFy_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + x_coeff = cy * cp; + y_coeff = cy * sp * sr - sy * cr; + z_coeff = cy * sp * cr + sy * sr; + double dFy_dY = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + + y_coeff = cp * cr; + z_coeff = -cp * sr; + double dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + double dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; + + x_coeff = -cp; + y_coeff = -sp * sr; + z_coeff = -sp * cr; + double dFz_dP = + (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + one_half_at_squared; + double dFY_dP = + (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; + + *state_tf_jacobian = state_tf; + (*state_tf_jacobian)(0, 3) = dFx_dR; + (*state_tf_jacobian)(0, 4) = dFx_dP; + (*state_tf_jacobian)(0, 5) = dFx_dY; + (*state_tf_jacobian)(1, 3) = dFy_dR; + (*state_tf_jacobian)(1, 4) = dFy_dP; + (*state_tf_jacobian)(1, 5) = dFy_dY; + (*state_tf_jacobian)(2, 3) = dFz_dR; + (*state_tf_jacobian)(2, 4) = dFz_dP; + (*state_tf_jacobian)(3, 3) = dFR_dR; + (*state_tf_jacobian)(3, 4) = dFR_dP; + (*state_tf_jacobian)(4, 3) = dFP_dR; + (*state_tf_jacobian)(5, 3) = dFY_dR; + (*state_tf_jacobian)(5, 4) = dFY_dP; + } + + // Predict the new state + fuse_core::Vector15d state; + state.head(3) = position; + state.segment(3,3) = fuse_core::Vector3d(roll, pitch, yaw); + state.segment(6,3) = vel_linear; + state.segment(9,3) = vel_angular; + state.tail(3) = acc_linear; + + state = state_tf * state; + + // Convert back + position_out = state.head(3); + fuse_core::wrapAngle2D(state(5)); + fuse_core::wrapAngle2D(state(4)); + fuse_core::wrapAngle2D(state(3)); + Eigen::Quaterniond q = + Eigen::AngleAxisd(state(5), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(state(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(state(3), Eigen::Vector3d::UnitX()); + orientation_out = q; + vel_linear_out = state.segment(6,3); + vel_angular_out = state.segment(9,3); + acc_linear_out = state.tail(3); +} + +template +inline void predict( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T dt, + T * const position2, + T * const orientation2, + T * const vel_linear2, + T * const vel_angular2, + T * const acc_linear2) +{ + // Creates some Eigen maps and pass them to the predict function + // First test just to try if this works, create new eigen vector and pass these to the predict function + Eigen::Matrix position_in(position1[0], position1[1], position1[2]); + Eigen::Quaternion orientation_in(orientation1[3], orientation1[0], orientation1[1], orientation1[2]); + Eigen::Matrix vel_linear_in(vel_linear1[0], vel_linear1[1], vel_linear1[2]); + Eigen::Matrix vel_angular_in(vel_angular1[0], vel_angular1[1], vel_angular1[2]); + Eigen::Matrix acc_linear_in(acc_linear1[0], acc_linear1[1], acc_linear1[2]); + Eigen::Matrix position_out, vel_linear_out, vel_angular_out, acc_linear_out; + Eigen::Quaternion orientation_out; + predict( + position_in, + orientation_in, + vel_linear_in, + vel_angular_in, + acc_linear_in, + dt, + position_out, + orientation_out, + vel_linear_out, + vel_angular_out, + acc_linear_out); + + // Convert back + position2[0] = position_out.x(); + position2[1] = position_out.y(); + position2[2] = position_out.z(); + orientation2[0] = orientation_out.x(); + orientation2[1] = orientation_out.y(); + orientation2[2] = orientation_out.z(); + orientation2[3] = orientation_out.w(); + vel_linear2[0] = vel_linear_out.x(); + vel_linear2[1] = vel_linear_out.y(); + vel_linear2[2] = vel_linear_out.z(); + vel_angular2[0] = vel_angular_out.x(); + vel_angular2[1] = vel_angular_out.y(); + vel_angular2[2] = vel_angular_out.z(); + acc_linear2[0] = acc_linear_out.x(); + acc_linear2[1] = acc_linear_out.y(); + acc_linear2[2] = acc_linear_out.z(); +} + +// /** +// * @brief Given a state and time delta, predicts a new state +// * @param[in] position1_x - First X position +// * @param[in] position1_y - First Y position +// * @param[in] position1_z - First Z position +// * @param[in] roll1 - First roll +// * @param[in] pitch1 - First pitch +// * @param[in] yaw1 - First yaw +// * @param[in] vel_linear1_x - First X velocity +// * @param[in] vel_linear1_y - First Y velocity +// * @param[in] vel_linear1_z - First Z velocity +// * @param[in] vel_roll1 - First roll velocity +// * @param[in] vel_pitch1 - First pitch velocity +// * @param[in] vel_yaw1 - First yaw velocity +// * @param[in] acc_linear1_x - First X acceleration +// * @param[in] acc_linear1_y - First Y acceleration +// * @param[in] acc_linear1_z - First Z acceleration +// * @param[in] dt - The time delta across which to predict the state +// * @param[out] position2_x - Second X position +// * @param[out] position2_y - Second Y position +// * @param[out] position2_z - Second Z position +// * @param[out] roll2 - Second roll +// * @param[out] pitch2 - Second pitch +// * @param[out] yaw2 - Second orientation +// * @param[out] vel_linear2_x - Second X velocity +// * @param[out] vel_linear2_y - Second Y velocity +// * @param[out] vel_linear2_z - Second Z velocity +// * @param[out] vel_roll2 - Second roll velocity +// * @param[out] vel_pitch2 - Second pitch velocity +// * @param[out] vel_yaw2 - Second yaw velocity +// * @param[out] acc_linear2_x - Second X acceleration +// * @param[out] acc_linear2_y - Second Y acceleration +// * @param[out] acc_linear2_z - Second Z acceleration +// * @param[out] jacobians - Jacobians wrt the state +// */ +// inline void predict( +// const double position1_x, +// const double position1_y, +// const double position1_z, +// // const double roll1, +// // const double pitch1, +// // const double yaw1, +// const double q1_x, +// const double q1_y, +// const double q1_z, +// const double q1_w, +// const double vel_linear1_x, +// const double vel_linear1_y, +// const double vel_linear1_z, +// const double vel_roll1, +// const double vel_pitch1, +// const double vel_yaw1, +// const double acc_linear1_x, +// const double acc_linear1_y, +// const double acc_linear1_z, +// const double dt, +// double & position2_x, +// double & position2_y, +// double & position2_z, +// // double & roll2, +// // double & pitch2, +// // double & yaw2, +// const double q2_x, +// const double q2_y, +// const double q2_z, +// const double q2_w, +// double & vel_linear2_x, +// double & vel_linear2_y, +// double & vel_linear2_z, +// double & vel_roll2, +// double & vel_pitch2, +// double & vel_yaw2, +// double & acc_linear2_x, +// double & acc_linear2_y, +// double & acc_linear2_z, +// double ** jacobians) +// { +// fuse_core::Vector3d position1(position1_x, position1_y, position1_z); +// // fuse_core::Vector3d orientation1(roll1, pitch1, yaw1); +// fuse_core::Quaternion orientation1(q1_w, q1_x, q1_y, q1_z); +// fuse_core::Vector3d vel_linear1(vel_linear1_x, vel_linear1_y, vel_linear1_z); +// fuse_core::Vector3d vel_angular1(vel_roll1, vel_pitch1, vel_yaw1); +// fuse_core::Vector3d acc_linear1(acc_linear1_x, acc_linear1_y, acc_linear1_z); +// fuse_core::Vector3d position2, orientation2, vel_linear2, vel_angular2, acc_linear2; +// fuse_core::Quaternion orientation2; +// fuse_core::Matrix15d* jacobian; + +// predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, +// position2, orientation2, vel_linear2, vel_angular2, acc_linear2, jacobian); + +// position2_x = position2.x(); +// position2_y = position2.y(); +// position2_z = position2.z(); +// // roll2 = orientation2.x(); +// // pitch2 = orientation2.y(); +// // yaw2 = orientation2.z(); +// q2_x = orientation2.x(); +// q2_y = orientation2.y(); +// q2_z = orientation2.z(); +// q2_w = orientation2.w(); +// vel_linear2_x = vel_linear2.x(); +// vel_linear2_y = vel_linear2.y(); +// vel_linear2_z = vel_linear2.z(); +// vel_roll2 = vel_angular2.x(); +// vel_pitch2 = vel_angular2.y(); +// vel_yaw2 = vel_angular2.z(); +// acc_linear2_x = acc_linear2.x(); +// acc_linear2_y = acc_linear2.y(); +// acc_linear2_z = acc_linear2.z(); + +// if (jacobians) +// { +// // Jacobian wrt position1 +// if (jacobians[0]) { +// Eigen::Map> j_temp(jacobians[0]); +// j_temp = jacobian->block<15, 3>(0, 0); +// } +// // Jacobian wrt orientation1 +// // if (jacobians[1]) { +// // Eigen::Map> j_temp(jacobians[1]); +// // j_temp = jacobian->block<15, 3>(0, 3); +// // } +// if (jacobians[1]) { +// Eigen::Map> j_temp(jacobians[1]); +// j_temp = jacobian->block<15, 3>(0, 3); +// } +// // Jacobian wrt vel_linear1 +// if (jacobians[2]) { +// Eigen::Map> j_temp(jacobians[2]); +// j_temp = jacobian->block<15, 3>(0, 6); +// } +// // Jacobian wrt vel_angular1 +// if (jacobians[3]) { +// Eigen::Map> j_temp(jacobians[3]); +// j_temp = jacobian->block<15, 3>(0, 9); +// } +// // Jacobian wrt acc_linear1 +// if (jacobians[4]) { +// Eigen::Map> j_temp(jacobians[4]); +// j_temp = jacobian->block<15, 3>(0, 12); +// } +// } +// } +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp new file mode 100644 index 000000000..d999f7f97 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp @@ -0,0 +1,301 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ + +#include + +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief Create a cost function for a 3D state vector + * + * The state vector includes the following quantities, given in this order: + * x position + * y position + * z position + * roll (rotation about the x axis) + * pitch (rotation about the y axis) + * yaw (rotation about the z axis) + * x velocity + * y velocity + * z velocity + * roll velocity + * pitch velocity + * yaw velocity + * x acceleration + * y acceleration + * z acceleration + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the entire state vector. + * + * The cost function is of the form: + * + * || [ x_t2 - proj(x_t1) ] ||^2 + * cost(x) = || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ roll_t2 - proj(roll_t1) ] || + * || [ pitch_t2 - proj(pitch_t1) ] || + * || [ yaw_t2 - proj(yaw_t1) ] || + * || A * [ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ z_vel_t2 - proj(z_vel_t1) ] || + * || [ roll_vel_t2 - proj(roll_vel_t1) ] || + * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ z_acc_t2 - proj(z_acc_t1) ] || + * + * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and + * proj is a function that projects the state variables from time t1 to time t2. In case the user is + * interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] dt The time delta across which to generate the kinematic model cost + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + Unicycle3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + * + * @param[in] parameters - Parameter blocks: + * 0 : position1 - First position (array with x at index 0, y at index 1, z at index 2) + * 1 : orientation1 - First orientation (array with roll at index 0, pitch at index 1, yaw at index 2) + * 2 : vel_linear1 - First linear velocity (array with vx at index 0, vy at index 1, + * vz at index 2) + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, + * z at index 2) + * 5 : position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * 6 : orientation2 - Second orientation (array with roll at index 0, pitch at index 1, yaw at index 2) + * 7 : vel_linear2 - Second linear velocity (array with vx at index 0, vy at index 1, + * vz at index 2) + * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, + * z at index 2) + * @param[out] residual - The computed residual (error) + * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not + * NULL, and only computed for the parameters where jacobians[i] is not + * NULL. + * @return The return value indicates whether the computation of the residuals and/or jacobians + * was successful or not. + */ + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + double position_pred_x, position_pred_y, position_pred_z; + double roll_pred, pitch_pred, yaw_pred; + double vel_linear_pred_x, vel_linear_pred_y, vel_linear_pred_z; + double vel_roll_pred, vel_pitch_pred, vel_yaw_pred; + double acc_linear_pred_x, acc_linear_pred_y, acc_linear_pred_z; + + predict( + parameters[0][0], parameters[0][2], parameters[0][2], + parameters[1][0], parameters[1][2], parameters[1][2], + parameters[2][0], parameters[2][2], parameters[2][2], + parameters[3][0], parameters[3][2], parameters[3][2], + parameters[4][0], parameters[4][2], parameters[4][2], + dt_, + position_pred_x, position_pred_y, position_pred_z, + roll_pred, pitch_pred, yaw_pred, + vel_linear_pred_x, vel_linear_pred_y, vel_linear_pred_z, + vel_roll_pred, vel_pitch_pred, vel_yaw_pred, + acc_linear_pred_x, acc_linear_pred_y, acc_linear_pred_z, + jacobians); + + residuals[0] = parameters[5][0] - position_pred_x; + residuals[1] = parameters[5][1] - position_pred_y; + residuals[2] = parameters[5][2] - position_pred_z; + residuals[3] = parameters[6][0] - roll_pred; + residuals[4] = parameters[6][1] - pitch_pred; + residuals[5] = parameters[6][2] - yaw_pred; + residuals[6] = parameters[7][0] - vel_linear_pred_x; + residuals[7] = parameters[7][1] - vel_linear_pred_y; + residuals[8] = parameters[7][2] - vel_linear_pred_z; + residuals[9] = parameters[8][0] - vel_roll_pred; + residuals[10] = parameters[8][1] - vel_pitch_pred; + residuals[11] = parameters[8][2] - vel_yaw_pred; + residuals[12] = parameters[9][0] - acc_linear_pred_x; + residuals[13] = parameters[9][1] - acc_linear_pred_y; + residuals[14] = parameters[9][2] - acc_linear_pred_z; + + fuse_core::wrapAngle2D(residuals[3]); + fuse_core::wrapAngle2D(residuals[4]); + fuse_core::wrapAngle2D(residuals[5]); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians) { + // It might be possible to simplify the code below implementing something like this but using + // compile-time template recursion. + // + // // state1: (position1, yaw1, vel_linear1, vel_yaw1, acc_linear1) + // for (size_t i = 0; i < 5; ++i) + // { + // if (jacobians[i]) + // { + // Eigen::Map> jacobian( + // jacobians[i]); + // jacobian.applyOnTheLeft(-A_); + // } + // } + + // Update jacobian wrt position1 + if (jacobians[0]) { + Eigen::Map> jacobian(jacobians[0]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt orientation + if (jacobians[1]) { + Eigen::Map> jacobian(jacobians[1]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt vel_linear1 + if (jacobians[2]) { + Eigen::Map> jacobian(jacobians[2]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt vel_angular1 + if (jacobians[3]) { + Eigen::Map> jacobian(jacobians[3]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt acc_linear1 + if (jacobians[4]) { + Eigen::Map> jacobian(jacobians[4]); + jacobian.applyOnTheLeft(-A_); + } + + // It might be possible to simplify the code below implementing something like this but using + // compile-time template recursion. + // + // // state2: (position2, yaw2, vel_linear2, vel_yaw2, acc_linear2) + // for (size_t i = 5, offset = 0; i < ParameterDims::kNumParameterBlocks; ++i) + // { + // constexpr auto dim = ParameterDims::GetDim(i); + + // if (jacobians[i]) + // { + // Eigen::Map> jacobian(jacobians[i]); + // jacobian = A_.block<8, dim>(0, offset); + // } + + // offset += dim; + // } + + // Jacobian wrt position2 + if (jacobians[5]) { + Eigen::Map> jacobian(jacobians[5]); + jacobian = A_.block<15, 3>(0, 0); + } + + // Jacobian wrt orientation2 + if (jacobians[6]) { + Eigen::Map> jacobian(jacobians[6]); + jacobian = A_.block<15, 3>(0, 3); + } + + // Jacobian wrt vel_linear2 + if (jacobians[7]) { + Eigen::Map> jacobian(jacobians[7]); + jacobian = A_.block<15, 3>(0, 6); + } + + // Jacobian wrt vel_angular2 + if (jacobians[8]) { + Eigen::Map> jacobian(jacobians[8]); + jacobian = A_.block<15, 3>(0, 9); + } + + // Jacobian wrt acc_linear2 + if (jacobians[9]) { + Eigen::Map> jacobian(jacobians[9]); + jacobian = A_.block<15, 3>(0, 12); + } + } + + return true; + } + +private: + double dt_; + fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix +}; + +Unicycle3DStateCostFunction::Unicycle3DStateCostFunction( + const double dt, + const fuse_core::Matrix15d & A) +: dt_(dt), + A_(A) +{ +} + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp new file mode 100644 index 000000000..5a9fee5c1 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -0,0 +1,204 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief Create a cost function for a 3D state vector + * + * The state vector includes the following quantities, given in this order: + * pose translation x, y, z + * pose orientation (in quaternion) x, y, z, w + * linear velocity x, y, z + * angular velocity x, y, z + * linear acceleration x, y, z + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the entire state vector. + * + * The cost function is of the form: + * + * || [ x_t2 - proj(x_t1) ] ||^2 + * cost(x) = || [ y_t2 - proj(y_t1) ] || + * || [ yaw_t2 - proj(yaw_t1) ] || + * ||A * [ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * + * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and + * proj is a function that projects the state variables from time t1 to time t2. In case the user is + * interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class Unicycle3DStateCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] dt The time delta across which to generate the kinematic model cost + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + */ + Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix16d & A); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation1 - First orientation (array with x at index 0, y at index 1, z at index 2, w at index 3) + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[in] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation2 - Second orientation (array with x at index 0, y at index 1, z at index 2, w at index 3) + * @param[in] vel_linear2 - Second linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular2 - Second angular velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[out] residual - The computed residual (error) + */ + template + bool operator()( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T * const position2, + const T * const orientation2, + const T * const vel_linear2, + const T * const vel_angular2, + const T * const acc_linear2, + T * residual) const; + +private: + double dt_; + fuse_core::Matrix16d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix +}; + +Unicycle3DStateCostFunctor::Unicycle3DStateCostFunctor( + const double dt, + const fuse_core::Matrix16d & A) +: dt_(dt), + A_(A) +{ +} + +template +bool Unicycle3DStateCostFunctor::operator()( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T * const position2, + const T * const orientation2, + const T * const vel_linear2, + const T * const vel_angular2, + const T * const acc_linear2, + T * residual) const +{ + T position_pred[3]; + T orientation_pred[4]; + T vel_linear_pred[3]; + T vel_angular_pred[3]; + T acc_linear_pred[3]; + predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + T(dt_), + position_pred, + orientation_pred, + vel_linear_pred, + vel_angular_pred, + acc_linear_pred); + + Eigen::Map> residuals_map(residual); + Eigen::Map> q_pred(orientation_pred); + Eigen::Quaternion q2(orientation2[3], orientation2[0], orientation2[1], orientation2[2]); + Eigen::Quaternion q_res; + // q_pred(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); + q_res = q2.inverse() * q_pred; + + residuals_map(0) = T(position2[0] - position_pred[0]); + residuals_map(1) = T(position2[1] - position_pred[1]); + residuals_map(2) = T(position2[2] - position_pred[2]); + residuals_map(3) = T(q_res.x()); + residuals_map(4) = T(q_res.y()); + residuals_map(5) = T(q_res.z()); + residuals_map(6) = T(q_res.w()); + residuals_map(7) = T(vel_linear2[0] - vel_linear_pred[0]); + residuals_map(8) = T(vel_linear2[1] - vel_linear_pred[1]); + residuals_map(9) = T(vel_linear2[2] - vel_linear_pred[2]); + residuals_map(10) = T(vel_angular2[0] - vel_angular_pred[0]); + residuals_map(11) = T(vel_angular2[1] - vel_angular_pred[1]); + residuals_map(12) = T(vel_angular2[2] - vel_angular_pred[2]); + residuals_map(13) = T(acc_linear2[0] - acc_linear_pred[0]); + residuals_map(14) = T(acc_linear2[1] - acc_linear_pred[1]); + residuals_map(15) = T(acc_linear2[2] - acc_linear_pred[2]); + + // This should be already done in the predict function + // fuse_core::wrapAngle2D(residuals_map(2)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + residuals_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp new file mode 100644 index 000000000..ee5b583c4 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief A class that represents a kinematic constraint between 3D states at two different times + * + * The fuse_models 3D state is a combination of 3D position, 3D orientation, 3D linear velocity, 3D + * angular velocity, and 3D linear acceleration. + */ +class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Unicycle3DStateKinematicConstraint) + + /** + * @brief Default constructor + */ + Unicycle3DStateKinematicConstraint() = default; + + /** + * @brief Create a constraint using a time delta and a kinematic model cost functor + * + * The constraint is created between two states. The state is broken up into multiple fuse + * variable types. + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 Position component variable of the fist state + * @param[in] orientation1 Orientation component variable of the first state + * @param[in] velocity_linear1 Linear velocity component variable of the first state + * @param[in] velocity_angular1 Angular velocity component variable of the first state + * @param[in] acceleration_linear1 Linear acceleration component variable of the first state + * @param[in] position2 Position component variable of the second state + * @param[in] orientation2 Orientation component variable of the second state + * @param[in] velocity_linear2 Linear velocity component variable of the second state + * @param[in] velocity_angular2 Angular velocity component variable of the second state + * @param[in] acceleration_linear2 Linear acceleration component variable of the second state + * @param[in] covariance - The covariance matrix used to weight the constraint. Order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + Unicycle3DStateKinematicConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::VelocityLinear3DStamped & velocity_linear1, + const fuse_variables::VelocityAngular3DStamped & velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_variables::VelocityLinear3DStamped & velocity_linear2, + const fuse_variables::VelocityAngular3DStamped & velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, + const fuse_core::Matrix16d & covariance); + + /** + * @brief Destructor + */ + virtual ~Unicycle3DStateKinematicConstraint() = default; + + /** + * @brief Read-only access to the time delta between the first and second state (really, between + * the position1 and position2 variables in the constructor) + */ + double dt() const {return dt_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + const fuse_core::Matrix16d & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + fuse_core::Matrix16d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + + /** + * @brief Print a human-readable description of the constraint 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 Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + double dt_; //!< The time delta for the constraint + fuse_core::Matrix16d sqrt_information_; //!< The square root information matrix + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & dt_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_KEY(fuse_models::Unicycle3DStateKinematicConstraint); + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 00f71e18a..4b235a458 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -174,6 +174,8 @@ bool Unicycle2D::applyCallback(fuse_core::Transaction & transaction) // manager, in turn, makes calls to the generateMotionModel() function. try { // Now actually generate the motion model segments + std::cout << "Transaction: " << std::endl; + transaction.print(std::cout); timestamp_manager_.query(transaction, true); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -309,6 +311,8 @@ void Unicycle2D::generateMotionModel( } else { state1 = base_state; } + // std::cout << "State1: " << state1 << std::endl; + state1.print(std::cout); // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); @@ -340,6 +344,9 @@ void Unicycle2D::generateMotionModel( state2.velocity_linear, state2.velocity_yaw, state2.acceleration_linear); + + // std::cout << "State2: " << state2 << std::endl; + state2.print(std::cout); // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp new file mode 100644 index 000000000..646366ee3 --- /dev/null +++ b/fuse_models/src/unicycle_3d.cpp @@ -0,0 +1,651 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3D, fuse_core::MotionModel) + +namespace std +{ + +inline bool isfinite(const fuse_core::Vector3d & vector) +{ + return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); +} + +inline bool isfinite(const fuse_core::Quaternion & quaternion) +{ + return std::isfinite(quaternion.x()) && std::isfinite(quaternion.y()) && + std::isfinite(quaternion.z()) && std::isfinite(quaternion.w()); +} + +std::string to_string(const fuse_core::Vector3d & vector) +{ + std::ostringstream oss; + oss << vector; + return oss.str(); +} + +std::string to_string(const fuse_core::Quaternion & quaternion) +{ + std::ostringstream oss; + oss << quaternion; + return oss.str(); +} + +} // namespace std + +namespace fuse_core +{ + +template +inline void validateCovariance( + const Eigen::DenseBase & covariance, + const double precision = Eigen::NumTraits::dummy_precision()) +{ + if (!fuse_core::isSymmetric(covariance, precision)) { + throw std::runtime_error( + "Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } + + if (!fuse_core::isPositiveDefinite(covariance)) { + throw std::runtime_error( + "Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } +} + +} // namespace fuse_core + +namespace fuse_models +{ + +Unicycle3D::Unicycle3D() +: fuse_core::AsyncMotionModel(1), // Thread count = 1 for local callback queue + logger_(rclcpp::get_logger("uninitialized")), + buffer_length_(rclcpp::Duration::max()), + device_id_(fuse_core::uuid::NIL), + timestamp_manager_(&Unicycle3D::generateMotionModel, this, rclcpp::Duration::max()) +{ +} + +void Unicycle3D::print(std::ostream & stream) const +{ + stream << "state history:\n"; + for (const auto & state : state_history_) { + stream << "- stamp: " << state.first.nanoseconds() << "\n"; + state.second.print(stream); + } +} + +void Unicycle3D::StateHistoryElement::print(std::ostream & stream) const +{ + stream << " position uuid: " << position_uuid << "\n" + << " orientation uuid: " << orientation_uuid << "\n" + << " velocity linear uuid: " << vel_linear_uuid << "\n" + << " velocity angular uuid: " << vel_angular_uuid << "\n" + << " acceleration linear uuid: " << acc_linear_uuid << "\n" + << " position: " << position << "\n" + << " orientation: " << orientation << "\n" + << " velocity linear: " << vel_linear << "\n" + << " velocity angular: " << vel_angular << "\n" + << " acceleration linear: " << acc_linear << "\n"; +} + +void Unicycle3D::StateHistoryElement::validate() const +{ + if (!std::isfinite(position)) { + throw std::runtime_error("Invalid position " + std::to_string(position)); + } + if (!std::isfinite(orientation)) { + throw std::runtime_error("Invalid orientation " + std::to_string(orientation)); + } + if (!std::isfinite(vel_linear)) { + throw std::runtime_error("Invalid linear velocity " + std::to_string(vel_linear)); + } + if (!std::isfinite(vel_angular)) { + throw std::runtime_error("Invalid angular velocity " + std::to_string(vel_angular)); + } + if (!std::isfinite(acc_linear)) { + throw std::runtime_error("Invalid linear acceleration " + std::to_string(acc_linear)); + } +} + +bool Unicycle3D::applyCallback(fuse_core::Transaction & transaction) +{ + // Use the timestamp manager to generate just the required motion model segments. The timestamp + // manager, in turn, makes calls to the generateMotionModel() function. + try { + // Now actually generate the motion model segments + timestamp_manager_.query(transaction, true); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); + return false; + } + return true; +} + +void Unicycle3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) +{ + updateStateHistoryEstimates(*graph, state_history_, buffer_length_); +} + +void Unicycle3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncMotionModel::initialize(interfaces, name); +} + +void Unicycle3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + std::vector process_noise_diagonal; + process_noise_diagonal = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "process_noise_diagonal"), + process_noise_diagonal); + + if (process_noise_diagonal.size() != 15) { + throw std::runtime_error("Process noise diagonal must be of length 15!"); + } + + process_noise_covariance_ = fuse_core::Vector15d(process_noise_diagonal.data()).asDiagonal(); + + // TODO: check these parameters + scale_process_noise_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "scale_process_noise"), + scale_process_noise_); + velocity_linear_norm_min_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "velocity_linear_norm_min"), + velocity_linear_norm_min_); + + velocity_angular_norm_min_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "velocity_angular_norm_min"), + velocity_angular_norm_min_); + + disable_checks_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "disable_checks"), + disable_checks_); + + double buffer_length = 3.0; + buffer_length = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "buffer_length"), + buffer_length); + + if (buffer_length < 0.0) { + throw std::runtime_error( + "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + } + + buffer_length_ = + (buffer_length == + 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + timestamp_manager_.bufferLength(buffer_length_); + + device_id_ = fuse_variables::loadDeviceId(interfaces_); +} + +void Unicycle3D::onStart() +{ + timestamp_manager_.clear(); + state_history_.clear(); +} + +void Unicycle3D::generateMotionModel( + const rclcpp::Time & beginning_stamp, + const rclcpp::Time & ending_stamp, + std::vector & constraints, + std::vector & variables) +{ + assert( + beginning_stamp < ending_stamp || + (beginning_stamp == ending_stamp && state_history_.empty())); + + StateHistoryElement base_state; + rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + + // Find an entry that is > beginning_stamp + // The entry that is <= will be the one before it + auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); + if (base_state_pair_it == state_history_.begin()) { + RCLCPP_WARN_STREAM_EXPRESSION( + logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + base_time = beginning_stamp; + } else { + --base_state_pair_it; + base_time = base_state_pair_it->first; + base_state = base_state_pair_it->second; + } + + StateHistoryElement state1; + + // If the nearest state we had was before the beginning stamp, we need to project that state to + // the beginning stamp + if (base_time != beginning_stamp) { + predict( + base_state.position, + base_state.orientation, + base_state.vel_linear, + base_state.vel_angular, + base_state.acc_linear, + (beginning_stamp - base_time).seconds(), + state1.position, + state1.orientation, + state1.vel_linear, + state1.vel_angular, + state1.acc_linear); + } else { + state1 = base_state; + } + std::cout << "state1:" << std::endl; + state1.print(std::cout); + + // If dt is zero, we only need to update the state history: + const double dt = (ending_stamp - beginning_stamp).seconds(); + + if (dt == 0.0) { + state1.position_uuid = fuse_variables::Position3DStamped(beginning_stamp, device_id_).uuid(); + state1.orientation_uuid = fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = + fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_angular_uuid = + fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = + fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); + state_history_.emplace(beginning_stamp, std::move(state1)); + return; + } + + // Now predict to get an initial guess for the state at the ending stamp + StateHistoryElement state2; + predict( + state1.position, + state1.orientation, + state1.vel_linear, + state1.vel_angular, + state1.acc_linear, + dt, + state2.position, + state2.orientation, + state2.vel_linear, + state2.vel_angular, + state2.acc_linear); + + std::cout << "state2:" << std::endl; + state2.print(std::cout); + + // Define the fuse variables required for this constraint + auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( + beginning_stamp, + device_id_); + auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared( + beginning_stamp, + device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared( + beginning_stamp, device_id_); + auto position2 = fuse_variables::Position3DStamped::make_shared(ending_stamp, device_id_); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(ending_stamp, device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared( + ending_stamp, + device_id_); + auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared( + ending_stamp, + device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared( + ending_stamp, + device_id_); + + position1->data()[fuse_variables::Position3DStamped::X] = state1.position.x(); + position1->data()[fuse_variables::Position3DStamped::Y] = state1.position.y(); + position1->data()[fuse_variables::Position3DStamped::Z] = state1.position.z(); + + // fuse_core::Quaternion q; + // q = Eigen::AngleAxisd(state1.orientation.x(), Eigen::Vector3d::UnitX()) * + // Eigen::AngleAxisd(state1.orientation.y(), Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(state1.orientation.z(), Eigen::Vector3d::UnitZ()); + + // orientation1->data()[fuse_variables::Orientation3DStamped::X] = q.x(); + // orientation1->data()[fuse_variables::Orientation3DStamped::Y] = q.y(); + // orientation1->data()[fuse_variables::Orientation3DStamped::Z] = q.z(); + // orientation1->data()[fuse_variables::Orientation3DStamped::W] = q.w(); + + orientation1->data()[fuse_variables::Orientation3DStamped::X] = state1.orientation.x(); + orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); + orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); + orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.orientation.w(); + + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.vel_linear.x(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.vel_linear.y(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.vel_linear.z(); + + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.vel_angular.y(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); + + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = + state1.acc_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = + state1.acc_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = + state1.acc_linear.z(); + + position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); + position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); + position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); + + // q = Eigen::AngleAxisd(state2.orientation.x(), Eigen::Vector3d::UnitX()) * + // Eigen::AngleAxisd(state2.orientation.y(), Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(state2.orientation.z(), Eigen::Vector3d::UnitZ()); +// + // orientation2->data()[fuse_variables::Orientation3DStamped::X] = q.x(); + // orientation2->data()[fuse_variables::Orientation3DStamped::Y] = q.y(); + // orientation2->data()[fuse_variables::Orientation3DStamped::Z] = q.z(); + // orientation2->data()[fuse_variables::Orientation3DStamped::Z] = q.w(); + + orientation2->data()[fuse_variables::Orientation3DStamped::X] = state2.orientation.x(); + orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); + orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); + orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.orientation.w(); + + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.vel_linear.x(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.vel_linear.y(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.vel_linear.z(); + + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.vel_angular.y(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); + + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = + state2.acc_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = + state2.acc_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = + state2.acc_linear.z(); + + state1.position_uuid = position1->uuid(); + state1.orientation_uuid = orientation1->uuid(); + state1.vel_linear_uuid = velocity_linear1->uuid(); + state1.vel_angular_uuid = velocity_angular1->uuid(); + state1.acc_linear_uuid = acceleration_linear1->uuid(); + + state2.position_uuid = position2->uuid(); + state2.orientation_uuid = orientation2->uuid(); + state2.vel_linear_uuid = velocity_linear2->uuid(); + state2.vel_angular_uuid = velocity_angular2->uuid(); + state2.acc_linear_uuid = acceleration_linear2->uuid(); + + state_history_.emplace(beginning_stamp, std::move(state1)); + state_history_.emplace(ending_stamp, std::move(state2)); + + // Scale process noise covariance pose by the norm of the current state twist + auto process_noise_covariance = process_noise_covariance_; + if (scale_process_noise_) { + common::scaleProcessNoiseCovariance( + process_noise_covariance, state1.vel_linear, state1.vel_angular, + velocity_linear_norm_min_, velocity_angular_norm_min_); + } + + // Validate + process_noise_covariance *= dt; + + if (!disable_checks_) { + try { + validateMotionModel(state1, state2, process_noise_covariance); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); + return; + } + } + + // Create the constraints for this motion model segment + auto constraint = fuse_models::Unicycle3DStateKinematicConstraint::make_shared( + name(), + *position1, + *orientation1, + *velocity_linear1, + *velocity_angular1, + *acceleration_linear1, + *position2, + *orientation2, + *velocity_linear2, + *velocity_angular2, + *acceleration_linear2, + process_noise_covariance); + + // Update the output variables + constraints.push_back(constraint); + variables.push_back(position1); + variables.push_back(orientation1); + variables.push_back(velocity_linear1); + variables.push_back(velocity_angular1); + variables.push_back(acceleration_linear1); + variables.push_back(position2); + variables.push_back(orientation2); + variables.push_back(velocity_linear2); + variables.push_back(velocity_angular2); + variables.push_back(acceleration_linear2); +} + +void Unicycle3D::updateStateHistoryEstimates( + const fuse_core::Graph & graph, + StateHistory & state_history, + const rclcpp::Duration & buffer_length) +{ + if (state_history.empty()) { + return; + } + + // Compute the expiration time carefully, as ROS can't handle negative times + const auto & ending_stamp = state_history.rbegin()->first; + + rclcpp::Time expiration_time; + if (ending_stamp.seconds() > buffer_length.seconds()) { + expiration_time = ending_stamp - buffer_length; + } else { + // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. + expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); + } + + // Remove state history elements before the expiration time. + // Be careful to ensure that: + // - at least one entry remains at all times + // - the history covers *at least* until the expiration time. Longer is acceptable. + auto expiration_iter = state_history.upper_bound(expiration_time); + if (expiration_iter != state_history.begin()) { + // expiration_iter points to the first element > expiration_time. + // Back up one entry, to a point that is <= expiration_time + state_history.erase(state_history.begin(), std::prev(expiration_iter)); + } + + // Update the states in the state history with information from the graph + // If a state is not in the graph yet, predict the state in question from the closest previous + // state + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); + ++current_iter) + { + const auto & current_stamp = current_iter->first; + auto & current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && + graph.variableExists(current_state.orientation_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && + graph.variableExists(current_state.vel_angular_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) + { + // This pose does exist in the graph. Update it directly. + const auto & position = graph.getVariable(current_state.position_uuid); + const auto & orientation = graph.getVariable(current_state.orientation_uuid); + const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto & vel_angular = graph.getVariable(current_state.vel_angular_uuid); + const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + + current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; + current_state.position.x() = position.data()[fuse_variables::Position3DStamped::Y]; + current_state.position.x() = position.data()[fuse_variables::Position3DStamped::Z]; + + current_state.orientation.x() = orientation.data()[fuse_variables::Orientation3DStamped::X]; + current_state.orientation.y() = orientation.data()[fuse_variables::Orientation3DStamped::Y]; + current_state.orientation.z() = orientation.data()[fuse_variables::Orientation3DStamped::Z]; + current_state.orientation.w() = orientation.data()[fuse_variables::Orientation3DStamped::W]; + + current_state.vel_linear.x() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; + current_state.vel_linear.y() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; + current_state.vel_linear.z() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; + + current_state.vel_angular.x() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; + current_state.vel_angular.y() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; + current_state.vel_angular.z() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; + + current_state.acc_linear.x() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; + current_state.acc_linear.y() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; + current_state.acc_linear.z() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; + } else if (current_iter != state_history.begin()) { + auto previous_iter = std::prev(current_iter); + const auto & previous_stamp = previous_iter->first; + const auto & previous_state = previous_iter->second; + + // This state is not in the graph yet, so we can't update/correct the value in our state + // history. However, the state *before* this one may have been corrected (or one of its + // predecessors may have been), so we can use that corrected value, along with our prediction + // logic, to provide a more accurate update to this state. + predict( + previous_state.position, + previous_state.orientation, + previous_state.vel_linear, + previous_state.vel_angular, + previous_state.acc_linear, + (current_stamp - previous_stamp).seconds(), + current_state.position, + current_state.orientation, + current_state.vel_linear, + current_state.vel_angular, + current_state.acc_linear); + } + } +} + +void Unicycle3D::validateMotionModel( + const StateHistoryElement & state1, const StateHistoryElement & state2, + const fuse_core::Matrix16d & process_noise_covariance) +{ + try { + state1.validate(); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); + } + + try { + state2.validate(); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); + } + // TODO: check validate covariance for 15x15 matrixes + try { + fuse_core::validateCovariance(process_noise_covariance); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); + } +} + +std::ostream & operator<<(std::ostream & stream, const Unicycle3D & unicycle_3d) +{ + unicycle_3d.print(stream); + return stream; +} + +} // namespace fuse_models diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp new file mode 100644 index 000000000..44e439c57 --- /dev/null +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -0,0 +1,408 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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. + */ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle2DIgnition, fuse_core::SensorModel); + +namespace fuse_models +{ + +Unicycle2DIgnition::Unicycle2DIgnition() +: fuse_core::AsyncSensorModel(1), + started_(false), + initial_transaction_sent_(false), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")) +{ +} + +void Unicycle2DIgnition::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Unicycle2DIgnition::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + // Connect to the reset service + if (!params_.reset_service.empty()) { + reset_client_ = rclcpp::create_client( + interfaces_.get_node_base_interface(), + interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), + params_.reset_service, + rclcpp::ServicesQoS(), + cb_group_ + ); + } + + // Advertise + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), + sub_options + ); + + set_pose_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName( + interfaces_.get_node_base_interface()->get_name(), + params_.set_pose_service), + std::bind( + &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), + cb_group_ + ); + set_pose_deprecated_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName( + interfaces_.get_node_base_interface()->get_name(), + params_.set_pose_deprecated_service), + std::bind( + &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), + cb_group_ + ); +} + +void Unicycle2DIgnition::start() +{ + started_ = true; + + // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once + // ever? I feel like it should be "only once ever". Send an initial state + // transaction immediately, if requested + if (params_.publish_on_startup && !initial_transaction_sent_) { + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); + pose.header.stamp = clock_->now(); + pose.pose.pose.position.x = params_.initial_state[0]; + pose.pose.pose.position.y = params_.initial_state[1]; + pose.pose.pose.orientation = + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); + pose.pose.covariance[0] = params_.initial_sigma[0] * params_.initial_sigma[0]; + pose.pose.covariance[7] = params_.initial_sigma[1] * params_.initial_sigma[1]; + pose.pose.covariance[35] = params_.initial_sigma[2] * params_.initial_sigma[2]; + sendPrior(pose); + initial_transaction_sent_ = true; + } +} + +void Unicycle2DIgnition::stop() +{ + started_ = false; +} + +void Unicycle2DIgnition::subscriberCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + try { + process(msg); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); + } +} + +bool Unicycle2DIgnition::setPoseServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) +{ + try { + process( + req->pose, + [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } catch (const std::exception & e) { + fuse_msgs::srv::SetPose::Response response; + response.success = false; + response.message = e.what(); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); + service->send_response(*request_id, response); + } + return true; +} + +bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) +{ + try { + process( + req->pose, + [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } catch (const std::exception & e) { + fuse_msgs::srv::SetPoseDeprecated::Response response; + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); + service->send_response(*request_id, response); + } + return true; +} + +void Unicycle2DIgnition::process( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +{ + // Verify we are in the correct state to process set pose requests + if (!started_) { + throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); + } + // Validate the requested pose and covariance before we do anything + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { + throw std::invalid_argument( + "Attempting to set the pose to an invalid position (" + + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ")."); + } + auto orientation_norm = std::sqrt( + pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + throw std::invalid_argument( + "Attempting to set the pose to an invalid orientation (" + + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); + } + auto position_cov = fuse_core::Matrix2d(); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], + pose.pose.covariance[6], pose.pose.covariance[7]; + if (!fuse_core::isSymmetric(position_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-symmetric position covariance matri\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + } + if (!fuse_core::isPositiveDefinite(position_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + } + auto orientation_cov = fuse_core::Matrix1d(); + orientation_cov << pose.pose.covariance[35]; + if (orientation_cov(0) <= 0.0) { + throw std::invalid_argument( + "Attempting to set the pose with a non-positive-definite orientation covariance " + "matrix " + fuse_core::to_string(orientation_cov) + "."); + } + + // Tell the optimizer to reset before providing the initial state + if (!params_.reset_service.empty()) { + // Wait for the reset service + while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && + interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM( + logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); + } + + auto srv = std::make_shared(); + // Don't block the executor. + // It needs to be free to handle the response to this service call. + // Have a callback do the rest of the work when a response comes. + auto result_future = reset_client_->async_send_request( + srv, + [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) { + post_process(); + } + }); + } else { + sendPrior(pose); + if (post_process) { + post_process(); + } + } +} + +void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + const auto & stamp = pose.header.stamp; + + // Create variables for the full state. + // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped + // message. The remaining dimensions are provided as parameters to the parameter server. + auto position = fuse_variables::Position2DStamped::make_shared(stamp, device_id_); + position->x() = pose.pose.pose.position.x; + position->y() = pose.pose.pose.position.y; + auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); + orientation->yaw() = fuse_core::getYaw( + pose.pose.pose.orientation.w, + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z); + auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); + linear_velocity->x() = params_.initial_state[3]; + linear_velocity->y() = params_.initial_state[4]; + auto angular_velocity = fuse_variables::VelocityAngular2DStamped::make_shared(stamp, device_id_); + angular_velocity->yaw() = params_.initial_state[5]; + auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared( + stamp, + device_id_); + linear_acceleration->x() = params_.initial_state[6]; + linear_acceleration->y() = params_.initial_state[7]; + + // Create the covariances for each variable + // The pose covariances are extracted from the provided PoseWithCovarianceStamped message. + // The remaining covariances are provided as parameters to the parameter server. + auto position_cov = fuse_core::Matrix2d(); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], + pose.pose.covariance[6], pose.pose.covariance[7]; + auto orientation_cov = fuse_core::Matrix1d(); + orientation_cov << pose.pose.covariance[35]; + auto linear_velocity_cov = fuse_core::Matrix2d(); + linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, + 0.0, params_.initial_sigma[4] * params_.initial_sigma[4]; + auto angular_velocity_cov = fuse_core::Matrix1d(); + angular_velocity_cov << params_.initial_sigma[5] * params_.initial_sigma[5]; + auto linear_acceleration_cov = fuse_core::Matrix2d(); + linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, + 0.0, params_.initial_sigma[7] * params_.initial_sigma[7]; + + // Create absolute constraints for each variable + auto position_constraint = fuse_constraints::AbsolutePosition2DStampedConstraint::make_shared( + name(), + *position, + fuse_core::Vector2d(position->x(), position->y()), + position_cov); + auto orientation_constraint = + fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + name(), + *orientation, + fuse_core::Vector1d(orientation->yaw()), + orientation_cov); + auto linear_velocity_constraint = + fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + name(), + *linear_velocity, + fuse_core::Vector2d(linear_velocity->x(), linear_velocity->y()), + linear_velocity_cov); + auto angular_velocity_constraint = + fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + name(), + *angular_velocity, + fuse_core::Vector1d(angular_velocity->yaw()), + angular_velocity_cov); + auto linear_acceleration_constraint = + fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + name(), + *linear_acceleration, + fuse_core::Vector2d(linear_acceleration->x(), linear_acceleration->y()), + linear_acceleration_cov); + + // Create the transaction + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(stamp); + transaction->addInvolvedStamp(stamp); + transaction->addVariable(position); + transaction->addVariable(orientation); + transaction->addVariable(linear_velocity); + transaction->addVariable(angular_velocity); + transaction->addVariable(linear_acceleration); + transaction->addConstraint(position_constraint); + transaction->addConstraint(orientation_constraint); + transaction->addConstraint(linear_velocity_constraint); + transaction->addConstraint(angular_velocity_constraint); + transaction->addConstraint(linear_acceleration_constraint); + + // Send the transaction to the optimizer. + sendTransaction(transaction); + + RCLCPP_INFO_STREAM( + logger_, + "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() + << ", x: " << position->x() << ", y: " + << position->y() << ", yaw: " << orientation->yaw() << + ")"); +} + +} // namespace fuse_models diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp new file mode 100644 index 000000000..88a4dbf33 --- /dev/null +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -0,0 +1,118 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include +#include +#include + +#include +// #include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_models +{ + +Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::VelocityLinear3DStamped & velocity_linear1, + const fuse_variables::VelocityAngular3DStamped & velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_variables::VelocityLinear3DStamped & velocity_linear2, + const fuse_variables::VelocityAngular3DStamped & velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, + const fuse_core::Matrix16d & covariance) +: fuse_core::Constraint( // TODO: check if there is a constructor for these arguments + source, + {position1.uuid(), + orientation1.uuid(), + velocity_linear1.uuid(), + velocity_angular1.uuid(), + acceleration_linear1.uuid(), + position2.uuid(), + orientation2.uuid(), + velocity_linear2.uuid(), + velocity_angular2.uuid(), + acceleration_linear2.uuid()}), // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +void Unicycle3DStateKinematicConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable 1: " << variables().at(0) << "\n" + << " orientation variable 1: " << variables().at(1) << "\n" + << " linear velocity variable 1: " << variables().at(2) << "\n" + << " angular velocity variable 1: " << variables().at(3) << "\n" + << " linear acceleration variable 1: " << variables().at(4) << "\n" + << " position variable 2: " << variables().at(5) << "\n" + << " orientation variable 2: " << variables().at(6) << "\n" + << " linear velocity variable 2: " << variables().at(7) << "\n" + << " angular velocity variable 2: " << variables().at(8) << "\n" + << " linear acceleration variable 2: " << variables().at(9) << "\n" + << " dt: " << dt() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * Unicycle3DStateKinematicConstraint::costFunction() const +{ + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + // + return new ceres::AutoDiffCostFunction( new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); + // + // which requires: + // + // #include + // return new Unicycle3DStateCostFunction(dt_, sqrt_information_); +} + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Unicycle3DStateKinematicConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DStateKinematicConstraint, fuse_core::Constraint); diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp new file mode 100644 index 000000000..6f1512dc5 --- /dev/null +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +// #include + +TEST(CostFunction, evaluateCostFunction) +{ + // Create cost function + const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); + + const double dt{0.1}; + const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; + + const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; + + // Evaluate cost function + const double position1[] = {0.0, 0.0, 0.0}; + const double orientation1[] = {0.0, 0.0, 0.0}; + const double vel_linear1[] = {1.0, 0.0}; + const double vel_yaw1[] = {1.570796327}; + const double acc_linear1[] = {1.0, 0.0}; + + const double position2[] = {0.105, 0.0}; + const double yaw2[] = {0.1570796327}; + const double vel_linear2[] = {1.1, 0.0}; + const double vel_yaw2[] = {1.570796327}; + const double acc_linear2[] = {1.0, 0.0}; + + const double * parameters[] = + { + position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 + }; + + fuse_core::Vector8d residuals; + + const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto num_parameter_blocks = block_sizes.size(); + + const auto num_residuals = cost_function.num_residuals(); + + std::vector J(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } + + EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); + + // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 + // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 + EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); + + // Check jacobians are correct using a gradient checker + ceres::NumericDiffOptions numeric_diff_options; + +#if CERES_VERSION_AT_LEAST(2, 1, 0) + std::vector parameterizations; + ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); +#else + ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +#endif + + // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error + // is 5.26356e-10 + ceres::GradientChecker::ProbeResults probe_results; + // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, + // but Probe actually returns true and the jacobians are correct according to the + // gradient checker numeric differentiation + // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << + // probe_results.error_log; + + // Create cost function using automatic differentiation on the cost functor + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + + // Evaluate cost function that uses automatic differentiation + std::vector J_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J_autodiff[i].resize(num_residuals, block_sizes[i]); + jacobians_autodiff[i] = J_autodiff[i].data(); + } + + EXPECT_TRUE( + cost_function_autodiff.Evaluate( + parameters, residuals.data(), + jacobians_autodiff.data())); + + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) + << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) + << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + } +} From ddd06c25d2ee8a17c3d523d0c92dfe97fcd455af Mon Sep 17 00:00:00 2001 From: giacomo Date: Fri, 3 Nov 2023 11:18:57 +0100 Subject: [PATCH 008/116] odometry 3d development --- fuse_constraints/CMakeLists.txt | 2 + fuse_constraints/fuse_plugins.xml | 36 +- .../fuse_constraints/absolute_constraint.hpp | 17 + ...olute_pose_3d_stamped_euler_constraint.hpp | 184 +++ ...rmal_delta_orientation_3d_cost_functor.hpp | 4 +- ...elta_orientation_3d_euler_cost_functor.hpp | 175 +++ ...ormal_delta_pose_3d_euler_cost_functor.hpp | 162 +++ ...rior_orientation_3d_euler_cost_functor.hpp | 53 +- ...ormal_prior_pose_3d_euler_cost_functor.hpp | 142 +++ ...ative_pose_3d_stamped_euler_constraint.hpp | 174 +++ fuse_constraints/src/absolute_constraint.cpp | 16 + .../absolute_pose_2d_stamped_constraint.cpp | 3 + ...olute_pose_3d_stamped_euler_constraint.cpp | 140 +++ ...ative_pose_3d_stamped_euler_constraint.cpp | 137 +++ fuse_core/include/fuse_core/eigen.hpp | 1 + fuse_models/CMakeLists.txt | 7 +- fuse_models/fuse_plugins.xml | 19 + .../fuse_models/common/sensor_config.hpp | 111 ++ .../fuse_models/common/sensor_proc.hpp | 578 ++++++++- .../fuse_models/common/variable_traits.hpp | 50 + .../include/fuse_models/odometry_3d.hpp | 185 +++ .../fuse_models/odometry_3d_publisher.hpp | 255 ++++ .../parameters/odometry_3d_params.hpp | 215 ++++ .../odometry_3d_publisher_params.hpp | 243 ++++ .../unicycle_3d_ignition_params.hpp | 218 ++++ .../include/fuse_models/unicycle_3d.hpp | 14 +- .../fuse_models/unicycle_3d_ignition.hpp | 10 +- .../fuse_models/unicycle_3d_predict.hpp | 1077 ++++++++--------- .../unicycle_3d_predict_no_vec.hpp | 311 +++++ .../unicycle_3d_state_cost_functor.hpp | 73 +- ...unicycle_3d_state_kinematic_constraint.hpp | 8 +- fuse_models/package.xml | 2 + fuse_models/src/odometry_3d.cpp | 230 ++++ fuse_models/src/odometry_3d_publisher.cpp | 644 ++++++++++ fuse_models/src/unicycle_2d.cpp | 9 +- ...unicycle_2d_state_kinematic_constraint.cpp | 10 +- fuse_models/src/unicycle_3d.cpp | 27 +- fuse_models/src/unicycle_3d_ignition.cpp | 210 ++-- ...unicycle_3d_state_kinematic_constraint.cpp | 5 +- fuse_models/test/CMakeLists.txt | 1 + fuse_models/test/test_unicycle_3d_predict.cpp | 214 ++++ .../test_unicycle_3d_state_cost_function.cpp | 70 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 1 - .../launch/range_sensor_tutorial.launch.py | 4 +- .../fuse_variables/orientation_3d_stamped.hpp | 7 + 45 files changed, 5249 insertions(+), 805 deletions(-) create mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_models/include/fuse_models/odometry_3d.hpp create mode 100644 fuse_models/include/fuse_models/odometry_3d_publisher.hpp create mode 100644 fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp create mode 100644 fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp create mode 100644 fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp create mode 100644 fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp create mode 100644 fuse_models/src/odometry_3d.cpp create mode 100644 fuse_models/src/odometry_3d_publisher.cpp create mode 100644 fuse_models/test/test_unicycle_3d_predict.cpp diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 660a7ee2e..4ab4ede2f 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp + src/absolute_pose_3d_stamped_euler_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -49,6 +50,7 @@ add_library(${PROJECT_NAME} src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp + src/relative_pose_3d_stamped_euler_constraint.cpp src/uuid_ordering.cpp src/variable_constraints.cpp ) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index aa6b02c54..b4c4bd7c6 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -5,12 +5,24 @@ the 2D angular acceleration. + + + A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of + the 2D angular acceleration. + + A constraint that represents either prior information about a 2D linear acceleration, or a direct measurement of the 2D linear acceleration. + + + A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of + the 2D linear acceleration. + + A constraint that represents either prior information about a 2D orientation, or a direct measurement of the @@ -35,12 +47,24 @@ the 2D angular velocity. + + + A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of + the 2D angular velocity. + + A constraint that represents either prior information about a 2D linear velocity, or a direct measurement of the 2D linear velocity. + + + A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of + the 2D linear velocity. + + A constraint that represents either prior information about a 3D orientation, or a direct measurement of the @@ -60,7 +84,12 @@ - A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose. + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose in quat. + + + + + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose in rpy. @@ -118,4 +147,9 @@ A constraint that represents a measurement on the difference between two 3D poses. + + + A constraint that represents a measurement on the difference between two 3D poses in rpy. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..a0189a9c2 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -46,11 +46,16 @@ #include #include #include +#include #include +#include #include +// #include #include #include #include +#include +#include #include #include @@ -196,14 +201,22 @@ class AbsoluteConstraint : public fuse_core::Constraint // Define unique names for the different variations of the absolute constraint using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationAngular3DStampedConstraint = + AbsoluteConstraint; using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear3DStampedConstraint = + AbsoluteConstraint; using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityAngular3DStampedConstraint = + AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = + AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; } // namespace fuse_constraints @@ -212,11 +225,15 @@ using AbsoluteVelocityLinear2DStampedConstraint = #include BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..1227f5829 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents either prior information about a 3D pose, or a direct + * measurement of the 3D pose. + * + * A 3D pose is the combination of a 3D position and a 3D orientation variable (in rpy). As a convenience, + * this class applies an absolute constraint on both variables at once. This type of constraint + * arises in many situations. In mapping it is common to define the very first pose as the origin. + * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. + * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map + * measurements). This constraint holds the measured 3D pose and the measurement + * uncertainty/covariance. Orientations are represented as quaternions. + */ +class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint) + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Default constructor + */ + AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior pose as a vector + * (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] covariance The measurement/prior covariance (max 6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] linear_indices The set of indices corresponding to the measured position + * dimensions e.g. "{fuse_variables::Position3DStamped::X, + * fuse_variables::Position3DStamped::Y, fuse_variables::Position3DStamped::Z}" + * @param[in] angular_indices The set of indices corresponding to the measured orientation + * dimensions e.g. "{fuse_variables::Orientation2DStamped::ROLL, + * fuse_variables::Orientation2DStamped::PITCH, fuse_variables::Orientation2DStamped::YAW}" + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector7d & mean, + const fuse_core::Matrix6d & covariance, + const std::vector & linear_indices, + const std::vector & angular_indices); + + /** + * @brief Destructor + */ + virtual ~AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, qw, qx, qy, qz) + */ + const fuse_core::Vector7d & mean() const {return mean_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint 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 Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + + + +protected: + fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + std::vector axes_; //!< The axes to use for the orientation error. + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & mean_; + archive & sqrt_information_; + archive & axes_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index a83e4479a..5fa881056 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -120,7 +120,9 @@ class NormalDeltaOrientation3DCostFunctor // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); + // TODO: Probably here we can set 3 as row dimension instead of Eigen::Dynamic + // Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map.applyOnTheLeft(A_.template cast()); return true; diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..f86ed9c7f --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp @@ -0,0 +1,175 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_HPP_ + +#include +#include + +#include +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D orientation variables + * (quaternion, but converted in RPY for residual computation) + * + * The cost function is of the form: + * + * || ||^2 + * cost(x) = || A * quat2rpy(b^-1 * q1^-1 * q2) || + * || || + * + * where the matrix A and the vector b are fixed, and q1 and q2 are the variables, represented as + * quaternions. The quat2rpy function converts a quaternion into a euler angles vector. + * The A matrix is applied to the euler angles vector. + * + * In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalDeltaOrientation3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z) + * @param[in] b The measured change between the two orientation variables (quaternion) + * @param[in] axes The axes to be considered in the cost function + */ + NormalDeltaOrientation3DEulerCostFunctor( + const fuse_core::Matrix3d & A, + const fuse_core::Vector4d & b, + std::vector axes) + : A_(A), + b_(b), + axes_(axes) + { + } + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T * const orientation1, const T * const orientation2, T * residuals) const + { + using fuse_variables::Orientation3DStamped; + + T orientation1_inverse[4] = + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + + T observation_inverse[4] = + { + T(b_(0)), + T(-b_(1)), + T(-b_(2)), + T(-b_(3)) + }; + + T difference[4]; + ceres::QuaternionProduct(orientation1_inverse, orientation2, difference); + T error[4]; + ceres::QuaternionProduct(observation_inverse, difference, error); + // instead of QuaternionToAngleAxis we implement QuaternionToRPY + // and filter the result based on the requested measurement axes + // ceres::QuaternionToAngleAxis(error, residuals); + + for (size_t i = 0; i < axes_.size(); ++i) { + T angle; + switch (axes_[i]) { + case Euler::ROLL: + { + angle = fuse_core::getRoll( + error[0], error[1], error[2], error[3]); + break; + } + case Euler::PITCH: + { + angle = + fuse_core::getPitch( + error[0], error[1], error[2], error[3]); + break; + } + case Euler::YAW: + { + angle = + fuse_core::getYaw( + error[0], error[1], error[2], error[3]); + break; + } + default: + { + throw std::runtime_error( + "The provided axis specified is unknown. " + "I should probably be more informative here"); + } + } + residuals[i] = angle; + } + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map.applyOnTheLeft(A_.template cast()); + + return true; + } + +private: + fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector4d b_; //!< The measured difference between orientation1 and orientation2 + std::vector axes_; //!< The axes to use for the orientation error. +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..f2b72b19a --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D pose variables with orientation in RPY. + * + * A single pose involves two variables: a 3D position and a 3D orientation. This cost function + * computes the difference using standard 3D transformation math: + * + * cost(x) = || A * [ q1^-1 * (p2 - p1) - b(0:2) ] ||^2 + * || [ quat2rpy(b(3:6)^-1 * q1^-1 * q2) ] || + * + * where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, + * and the matrix A and the vector b are fixed. In case the user is interested in implementing a + * cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + * + * Note that the cost function's quaternion components are only concerned with the imaginary + * components (qx, qy, qz). + */ +class NormalDeltaPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Constructor + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (dx, dy, dz, dqx, dqy, dqz) + * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] axes The axes to use for the orientation error. Defaults to all three axes. + */ + NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, + const fuse_core::Vector7d & b, + const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); + + /** + * @brief Compute the cost values/residuals using the provided variable/parameter values + */ + template + bool operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector7d b_; //!< The measured difference between variable pose1 and variable pose2 + NormalDeltaOrientation3DEulerCostFunctor orientation_functor_; +}; + +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector7d & b, + const std::vector & axes) +: A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>(), axes) // Orientation residuals will + // not be scaled +{ +} + +template +bool NormalDeltaPose3DEulerCostFunctor::operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const +{ + // Compute the position delta between pose1 and pose2 + T orientation1_inverse[4] = + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + T position_delta[3] = + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + T position_delta_rotated[3]; + ceres::QuaternionRotatePoint( + orientation1_inverse, + position_delta, + position_delta_rotated); + + // Compute the first three residual terms as (position_delta - b) + residual[0] = position_delta_rotated[0] - T(b_[0]); + residual[1] = position_delta_rotated[1] - T(b_[1]); + residual[2] = position_delta_rotated[2] - T(b_[2]); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation1, orientation2, &residual[3]); + + // Map it to Eigen, and weight it + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index fd49f06c5..e91a6a59b 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -83,12 +83,12 @@ class NormalPriorOrientation3DEulerCostFunctor * * @param[in] A The residual weighting matrix, most likely the square root information matrix. Its * order must match the values in \p axes. - * @param[in] b The orientation measurement or prior. Its order must match the values in \p axes. + * @param[in] b The orientation measurement or prior. * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. */ NormalPriorOrientation3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::VectorXd & b, + const fuse_core::Matrix3d & A, + const fuse_core::Vector4d & b, const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}) : //NOLINT A_(A), b_(b), @@ -104,26 +104,46 @@ class NormalPriorOrientation3DEulerCostFunctor { using fuse_variables::Orientation3DStamped; + // Compute the delta quaternion + // T variable[4] = + // { + // orientation[0], + // orientation[1], + // orientation[2], + // orientation[3] + // }; + + T observation_inverse[4] = + { + T(b_(0)), + T(-b_(1)), + T(-b_(2)), + T(-b_(3)) + }; + + T difference[4]; + ceres::QuaternionProduct(observation_inverse, orientation, difference); + + T angle[3] = {T(0.0), T(0.0), T(0.0)}; + for (size_t i = 0; i < axes_.size(); ++i) { - T angle; switch (axes_[i]) { case Euler::ROLL: { - angle = fuse_core::getRoll( - orientation[0], orientation[1], orientation[2], - orientation[3]); + angle[0] = fuse_core::getRoll( + difference[0], difference[1], difference[2], difference[3]); break; } case Euler::PITCH: { - angle = - fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); + angle[1] = fuse_core::getPitch( + difference[0], difference[1], difference[2], difference[3]); break; } case Euler::YAW: { - angle = - fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); + angle[2] = fuse_core::getYaw( + difference[0], difference[1], difference[2], difference[3]); break; } default: @@ -133,19 +153,22 @@ class NormalPriorOrientation3DEulerCostFunctor "I should probably be more informative here"); } } - residuals[i] = angle - T(b_[i]); } - Eigen::Map> residuals_map(residuals, A_.rows()); + residuals[0] = angle[0]; + residuals[1] = angle[1]; + residuals[2] = angle[2]; + + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map.applyOnTheLeft(A_.template cast()); return true; } private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix - fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value + fuse_core::Vector4d b_; //!< The measured 3D orientation (quaternion) value std::vector axes_; //!< The Euler angle axes that we're measuring }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..51bbbd186 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the 3D position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the 3D position and orientation variables at + * once. + * + * The cost function is of the form: + * + * cost(x) = || A * [ p - b(0:2) ] ||^2 + * || [ rpy - b(3:6) ] || + * + * where, the matrix A and the vector b are fixed, p is the position variable, and rpy is the + * orientation variable. In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + * @param[in] axes The axes to use for the orientation error. Defaults to all three axes. + */ + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, + const fuse_core::Vector7d & b, + const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T * const position, const T * const orientation, T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector7d b_; + + NormalPriorOrientation3DEulerCostFunctor orientation_functor_; +}; + +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector7d & b, + const std::vector & axes) +: A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>(), axes) // Delta will not be scaled +{ +} + +template +bool NormalPriorPose3DEulerCostFunctor::operator()( + const T * const position, const T * const orientation, + T * residual) const +{ + // Compute the position error + residual[0] = position[0] - T(b_(0)); + residual[1] = position[1] - T(b_(1)); + residual[2] = position[2] - T(b_(2)); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation, &residual[3]); + // for (size_t i = 0; i < 6; ++i) + // { + // std::cout << "residuals before scaling" << std::endl; + // std::cout << residual[i] << std::endl; + // } + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); + // for (size_t i = 0; i < 6; ++i) + // { + // std::cout << "residuals after scaling" << std::endl; + // std::cout << residual[i] << std::endl; + // } + // std::cout << std::endl; + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..6ead861a4 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,174 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents a measurement on the difference between two 3D poses + * with orientation in rpy. + * + * This type of constraint arises in many situations. Many types of incremental odometry + * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This + * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. + */ +class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedEulerConstraint) + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Default constructor + */ + RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Constructor + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) + * @param[in] linear_indices The indices of the linear variables in the variable vector + * @param[in] angular_indices The indices of the angular variables in the variable vector + */ + RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::VectorXd & delta, + const fuse_core::MatrixXd & covariance, + const std::vector & linear_indices, + const std::vector & angular_indices); + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + */ + const fuse_core::Vector7d & delta() const {return delta_;} + + /** + * @brief Read-only access to the square root information matrix. + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint 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 Access the cost function for this constraint + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the + //!< covariance matrix) + std::vector axes_; //!< The axes to use for the orientation error. + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & delta_; + archive & sqrt_information_; + archive & axes_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..bf4ad6519 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -36,19 +36,29 @@ #include BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); @@ -61,6 +71,12 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, + fuse_core::Constraint); \ No newline at end of file diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 1b84d5f1b..7fcdebae2 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -64,6 +64,7 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( // Compute the sqrt information of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; // Assemble a mean vector and sqrt information matrix from the provided values, but in proper // Variable order @@ -85,6 +86,8 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( mean_(final_index) = partial_mean(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); } + std::cout << "mean_: \n" << mean_.transpose() << std::endl; + std::cout << "sqrt_information_: \n" << sqrt_information_ << std::endl; } fuse_core::Matrix3d AbsolutePose2DStampedConstraint::covariance() const diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..9a3eddff5 --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace fuse_constraints +{ + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector7d & mean, + const fuse_core::Matrix6d & covariance, + const std::vector & linear_indices, + const std::vector & angular_indices) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT +{ + axes_.resize(angular_indices.size()); + std::transform( + angular_indices.begin(), angular_indices.end(), axes_.begin(), + [](const size_t index) + {return static_cast(index + 1UL);}); + + // merge indices + std::vector total_indices; + total_indices.reserve(linear_indices.size() + angular_indices.size()); + std::copy(linear_indices.begin(), linear_indices.end(), std::back_inserter(total_indices)); + std::copy(angular_indices.begin(), angular_indices.end(), std::back_inserter(total_indices)); + + // Compute the sqrt information of the provided cov matrix + fuse_core::Matrix6d sqrt_information = covariance.inverse().llt().matrixU(); + // std::cout << "sqrt_information = " << "\n" << sqrt_information << std::endl; + + // Assemble a mean vector and sqrt information matrix from the provided values, but in proper + // Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // build partial mean and information matrix + mean_ = fuse_core::Vector7d::Zero(); + sqrt_information_ = fuse_core::Matrix6d::Zero(); + + for (size_t i = 0; i < linear_indices.size(); ++i) { + mean_(linear_indices[i]) = mean(linear_indices[i]); + } + + mean_.tail(4) = mean.tail(4); + + for (auto& i : total_indices) { + sqrt_information_.col(i) = sqrt_information.col(i); + } + + // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; + // std::cout << "mean_ = " << mean_.transpose() << std::endl; +} + +fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const +{ + // We want to compute: + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable: " << variables().at(0) << "\n" + << " orientation variable: " << variables().at(1) << "\n" + << " mean: " << mean().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; + + if (loss()) { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_, axes_)); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..10317e0c7 --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include + +#include + +#include +#include +#include +#include + +namespace fuse_constraints +{ + +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::VectorXd & delta, + const fuse_core::MatrixXd & covariance, + const std::vector & linear_indices, + const std::vector & angular_indices) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}) // NOLINT +{ + axes_.resize(angular_indices.size()); + std::transform( + angular_indices.begin(), angular_indices.end(), axes_.begin(), + [](const size_t index) + {return static_cast(index + 3UL);}); + size_t total_variable_size = position1.size() + orientation1.size(); + size_t total_indices = linear_indices.size() + angular_indices.size(); + + assert(delta.rows() == static_cast(total_indices)); + assert(covariance.rows() == static_cast(total_indices)); + assert(covariance.cols() == static_cast(total_indices)); + + // Compute the sqrt information of the provided cov matrix + fuse_core::MatrixXd sqrt_information = covariance.inverse().llt().matrixU(); + + // Assemble a delta vector and sqrt information matrix from the provided values, but in proper + // Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + delta_ = fuse_core::Vector7d::Zero(); + sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); + for (size_t i = 0; i < linear_indices.size(); ++i) { + // build the partial translation delta and sqrt information + delta_(linear_indices[i]) = delta(i); + sqrt_information_.col(linear_indices[i]) = sqrt_information.col(i); + } + + // copy the full quaternion into the delta vector + delta_.tail(4) = delta.tail(4); + + for (size_t i = linear_indices.size(); i < total_indices; ++i) { + size_t final_index = position1.size() + angular_indices[i - linear_indices.size()]; + // build the partial rotation sqrt information + sqrt_information_.col(final_index) = sqrt_information.col(i); + } +} + +fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const +{ + // We want to compute: + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position1 variable: " << variables().at(0) << "\n" + << " orientation1 variable: " << variables().at(1) << "\n" + << " position2 variable: " << variables().at(2) << "\n" + << " orientation2 variable: " << variables().at(3) << "\n" + << " delta: " << delta().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_, axes_)); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 905ecb081..93bf334df 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -55,6 +55,7 @@ using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; using Vector15d = Eigen::Matrix; +using Vector16d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index dbe3c8574..12509630f 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake_ros REQUIRED) +find_package(covariance_geometry_ros REQUIRED) find_package(fuse_constraints REQUIRED) find_package(fuse_core REQUIRED) find_package(fuse_graphs REQUIRED) @@ -45,6 +46,8 @@ add_library(${PROJECT_NAME} src/imu_2d.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp + src/odometry_3d.cpp + src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp src/twist_2d.cpp @@ -52,7 +55,7 @@ add_library(${PROJECT_NAME} src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp src/unicycle_3d.cpp - # src/unicycle_3d_ignition.cpp + src/unicycle_3d_ignition.cpp src/unicycle_3d_state_kinematic_constraint.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -60,6 +63,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" ) target_link_libraries(${PROJECT_NAME} + covariance_geometry_ros::covariance_geometry_ros Ceres::ceres fuse_constraints::fuse_constraints fuse_core::fuse_core @@ -118,6 +122,7 @@ ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( ament_cmake_ros + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 809c4c429..4c3120686 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -33,6 +33,14 @@ + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 3D + state data (combination of Position3DStamped, Orientation3DStamped, VelocityLinear3DStamped, and + VelocityAngular3DStamped). + + + An adapter-type sensor that produces 2D linear acceleration constraints from information published by @@ -59,6 +67,12 @@ published by another node + + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data + published by another node + + An adapter-type sensor that produces absolute or relative pose constraints from information published by @@ -81,4 +95,9 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. + + + A fuse_models ignition sensor designed to be used in conjunction with the unicycle 3D motion model. + + diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b9f2b3505..8c8ed84de 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -44,10 +44,14 @@ #include #include +#include #include +#include #include #include +#include #include +#include #include @@ -90,6 +94,28 @@ std::enable_if_t::value, size_t> toIndex(const std::string & dim return 0u; } +/** + * @brief Method that converts from 3D linear axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D linear quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "x") {return static_cast(T::X);} + if (lower_dim == "y") {return static_cast(T::Y);} + if (lower_dim == "z") {return static_cast(T::Z);} + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Method that converts from 2D angular axis dimension names to index values * @@ -112,6 +138,91 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di return 0u; } +/** + * @brief Method that converts from 3D angular axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D angular quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(T::Euler::ROLL) - 1UL; + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(T::Euler::PITCH) - 1UL; + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(T::Euler::YAW) - 1UL; + } + + throwDimensionError(dimension); + + return 0u; +} + +/** + * @brief Method that converts from 3D angular axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D angular quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(T::ROLL); + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(T::PITCH); + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(T::YAW); + } + + throwDimensionError(dimension); + + return 0u; +} + +// /** +// * @brief Method that converts from 3D angular axis dimension names to index values +// * +// * This method is enabled only for variable Orientation3DStamped +// * +// * @param[in] dimension - The dimension name to convert +// * @return the index of the enumerated dimension for that type +// * @throws runtime_error if the dimension name is invalid +// */ +// template<> +// std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +// { +// auto lower_dim = boost::algorithm::to_lower_copy(dimension); +// if (lower_dim == "roll" || lower_dim == "x") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 3UL; +// // Trick to obtain indices of roll, pitch, yaw as 3, 4, 5 relative to 3d pose_mean +// } +// if (lower_dim == "pitch" || lower_dim == "y") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 3UL; +// } +// if (lower_dim == "yaw" || lower_dim == "z") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 3UL; +// } + +// throwDimensionError(dimension); + +// return 0u; +// } + /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 4a2f317ed..462c337b7 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -46,7 +46,11 @@ #include #include +#include +#include #include +#include +#include #include #include #include @@ -69,6 +73,10 @@ #include #include +#include "covariance_geometry/pose_covariance_representation.hpp" +#include "covariance_geometry/pose_covariance_composition.hpp" +#include "covariance_geometry_ros/utils.hpp" + #include @@ -167,6 +175,7 @@ inline std::vector mergeIndices( return merged_indices; } +// TODO: is possible to enlarge this to work with eigen maps and view of those? /** * @brief Method to create sub-measurements from full measurements and append them to existing * partial measurements @@ -194,6 +203,31 @@ inline void populatePartialMeasurement( } } +/** + * @brief Method to create sub-measurements from full measurements and append them to existing + * partial measurements - version with covariance only + * + * @param[in] covariance_full - The full covariance matrix from which we will generate the sub- + * measurement + * @param[in] indices - The indices we want to include in the sub-measurement + * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append + */ +inline void populatePartialMeasurement( + const fuse_core::MatrixXd & covariance_full, + const std::vector & indices, + fuse_core::MatrixXd & covariance_partial) +{ + covariance_partial.setZero(); + for (auto & i : indices) { + covariance_partial.col(i) = covariance_full.col(i); + } + // for (size_t r = 0; r < indices.size(); ++r) { + // for (size_t c = 0; c < indices.size(); ++c) { + // covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + // } + // } +} + /** * @brief Method to validate partial measurements, that checks for finite values and covariance * properties @@ -224,6 +258,28 @@ inline void validatePartialMeasurement( } } +inline void validateMeasurement( + const fuse_core::VectorXd & mean, + const fuse_core::MatrixXd & covariance, + const double precision = Eigen::NumTraits::dummy_precision()) +{ + if (!mean.allFinite()) { + throw std::runtime_error("Invalid mean " + fuse_core::to_string(mean)); + } + + if (!fuse_core::isSymmetric(covariance, precision)) { + throw std::runtime_error( + "Non-symmetric covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } + + if (!fuse_core::isPositiveDefinite(covariance)) { + throw std::runtime_error( + "Non-positive-definite covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } +} + /** * @brief Transforms a ROS geometry message from its frame to the frame of the output message * @@ -357,7 +413,7 @@ inline bool processAbsolutePoseWithCovariance( if (validate) { try { - validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-4); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -387,6 +443,125 @@ inline bool processAbsolutePoseWithCovariance( return true; } +/** + * @brief Extracts 3D pose data from a PoseWithCovarianceStamped message and adds that data to a + * fuse Transaction + * + * This method effectively adds two variables (3D position and 3D orientation) and a 3D pose + * constraint to the given \p transaction. The pose data is extracted from the \p pose message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose - The PoseWithCovarianceStamped message from which we will extract the pose data + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] target_frame - The frame ID into which the pose data will be transformed before it is + * used + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAbsolutePose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & position_indices, + const std::vector & orientation_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = pose; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + + // Create the pose variable + auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); + auto orientation = + fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + position->x() = pose.pose.pose.position.x; + position->y() = pose.pose.pose.position.y; + position->z() = pose.pose.pose.position.z; + orientation->x() = pose.pose.pose.orientation.x; + orientation->y() = pose.pose.pose.orientation.y; + orientation->z() = pose.pose.pose.orientation.z; + orientation->w() = pose.pose.pose.orientation.w; + + + // Create the pose for the constraint + fuse_core::Vector7d pose_mean; + pose_mean << pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.position.z, + pose.pose.pose.orientation.w, pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z; + + // Create the covariance for the constraint + Eigen::Map cov_map(pose.pose.covariance.data()); + fuse_core::Matrix6d pose_covariance = cov_map; // TODO check how to avoid this to not doing copies + + if (validate) { + try { + validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + std::cout << "AbsolutePose3DStampedEulerConstraint creating..." << std::endl; + + // Create an absolute pose constraint + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( + source, + *position, + *orientation, + pose_mean, + pose_covariance, + position_indices, + orientation_indices); + + // auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( + // source, + // *position, + // *orientation, + // pose_mean, + // pose_covariance); + + std::cout << "AbsolutePose3DStampedEulerConstraint created" << std::endl; + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -750,6 +925,198 @@ inline bool processDifferentialPoseWithCovariance( return true; } +/** + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction + * + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. The pose delta is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the covariance of each pose message is rotated into the robot's base frame at the + * time of pose_absolute1. They are then added in the constraint if the pose measurements are + * independent. Otherwise, if the pose measurements are dependent, the covariance of pose_absolute1 + * is substracted from the covariance of pose_absolute2. A small minimum relative covariance is + * added to avoid getting a zero or ill-conditioned covariance. This could happen if both covariance + * matrices are the same or very similar, e.g. when pose_absolute1 == pose_absolute2, it's possible + * that the covariance is the same for both poses. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always + * added to the resulting pose relative covariance + * @param[in] loss - The loss function for the 2D pose constraint generated + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processDifferentialPose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, + const fuse_core::Matrix6d & minimum_pose_relative_covariance, + const fuse_core::Loss::SharedPtr & loss, + const std::vector & position_indices, + const std::vector & orientation_indices, + const bool validate, + fuse_core::Transaction & transaction) +{ + // Better to convert right now in covariance_geometry types, since we need to compute relative poses + // and covariances. + // PoseQuaternionCovarianceRPY is std::pair, Covariance> + // Position is Eigen::Vector3d + // Quaternion is Eigen::Quaterniond + // Covariance is Eigen::Matrix6d + + covariance_geometry::PoseQuaternionCovarianceRPY pose1_3d, pose2_3d; + covariance_geometry::fromROS(pose1.pose, pose1_3d); + covariance_geometry::fromROS(pose2.pose, pose2_3d); + + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1_3d.first.first.x(); + position1->y() = pose1_3d.first.first.y(); + position1->z() = pose1_3d.first.first.z(); + orientation1->x() = pose1_3d.first.second.x(); + orientation1->y() = pose1_3d.first.second.y(); + orientation1->z() = pose1_3d.first.second.z(); + orientation1->w() = pose1_3d.first.second.w(); + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2_3d.first.first.x(); + position2->y() = pose2_3d.first.first.y(); + position2->z() = pose2_3d.first.first.z(); + orientation2->x() = pose2_3d.first.second.x(); + orientation2->y() = pose2_3d.first.second.y(); + orientation2->z() = pose2_3d.first.second.z(); + orientation2->w() = pose2_3d.first.second.w(); + + // Create the delta for the constraint and the relative covariance + // Technically there should be no differences between dependent and independent poses + covariance_geometry::PoseQuaternionCovarianceRPY pose_relative_3d, pose1_inv_3d; + pose1_inv_3d = covariance_geometry::inversePose3DQuaternionCovarianceRPY(pose1_3d); + covariance_geometry::ComposePoseQuaternionCovarianceRPY( + pose1_inv_3d, pose2_3d, pose_relative_3d); + + // if (orientation_indices.size() < 3u) { + // // We don't use all the orientations, so we switch to rpy representation + // covariance_geometry::PoseRPYCovariance pose_relative_3d_rpy; + // covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( + // pose_relative_3d, pose_relative_3d_rpy); + + // fuse_core::Vector6d pose_relative_mean; + // pose_relative_mean << pose_relative_3d_rpy.first.first, pose_relative_3d_rpy.first.second; + // fuse_core::Matrix6d pose_relative_covariance; + // pose_relative_covariance = pose_relative_3d_rpy.second + minimum_pose_relative_covariance; + + // // Build the sub-vector and sub-matrices based on the requested indices + // fuse_core::VectorXd pose_relative_mean_partial( + // position_indices.size() + orientation_indices.size()); + // fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), + // pose_relative_mean_partial.rows()); + + // const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); + + // populatePartialMeasurement( + // pose_relative_mean, + // pose_relative_covariance, + // indices, + // pose_relative_mean_partial, + // pose_relative_covariance_partial); + + // if (validate) { + // try { + // validatePartialMeasurement( + // pose_relative_mean_partial, pose_relative_covariance_partial, + // 1e-6); + // } catch (const std::runtime_error & ex) { + // RCLCPP_ERROR_STREAM_THROTTLE( + // rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + // "Invalid partial differential pose measurement from '" + // << source << "' source: " << ex.what()); + // return false; + // } + // } + // // Create a relative pose constraint. + // // TODO: implement relative pose constraint with orientation in rpy + // auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + // source, + // *position1, + // *orientation1, + // *position2, + // *orientation2, + // pose_relative_mean, + // pose_relative_covariance); + + // constraint->loss(loss); + + // transaction.addVariable(position1); + // transaction.addVariable(orientation1); + // transaction.addVariable(position2); + // transaction.addVariable(orientation2); + // transaction.addConstraint(constraint); + // transaction.addInvolvedStamp(pose1.header.stamp); + // transaction.addInvolvedStamp(pose2.header.stamp); + + // return true; + // } + + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << pose_relative_3d.first.first.x(), pose_relative_3d.first.first.y(), + pose_relative_3d.first.first.z(), pose_relative_3d.first.second.w(), + pose_relative_3d.first.second.x(), pose_relative_3d.first.second.y(), + pose_relative_3d.first.second.z(); + fuse_core::Matrix6d pose_relative_covariance; + pose_relative_covariance = pose_relative_3d.second + minimum_pose_relative_covariance; + + if (validate) { + try { + validateMeasurement( + pose_relative_mean, pose_relative_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance, + position_indices, + orientation_indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -1114,6 +1481,194 @@ inline bool processTwistWithCovariance( return constraints_added; } +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse + * Transaction + * + * This method effectively adds two variables (3D linear velocity and 3D angular velocity) and their + * respective constraints to the given \p transaction. The velocity data is extracted from the \p + * twist message. The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist + * data + * @param[in] linear_velocity_loss - The loss function for the 3D linear velocity constraint + * generated + * @param[in] angular_velocity_loss - The loss function for the 3D angular velocity constraint + * generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is + * used + * @param[in] linear_indices - The indices of the linear velocity vector to use. If empty, no + * linear velocity constraint is added + * @param[in] angular_indices - The indices of the angular velocity vector to use. If empty, no + * angular velocity constraint is added + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processTwist3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const fuse_core::Loss::SharedPtr & linear_velocity_loss, + const fuse_core::Loss::SharedPtr & angular_velocity_loss, + const std::string & target_frame, + const std::vector & linear_indices, + const std::vector & angular_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (linear_indices.empty() && angular_indices.empty()) { + return false; + } + + geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = twist; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + + bool constraints_added = false; + + // Create two absolute constraints + if (!linear_indices.empty()) { + auto velocity_linear = + fuse_variables::VelocityLinear3DStamped::make_shared(twist.header.stamp, device_id); + velocity_linear->x() = transformed_message.twist.twist.linear.x; + velocity_linear->y() = transformed_message.twist.twist.linear.y; + velocity_linear->z() = transformed_message.twist.twist.linear.z; + + // Create the mean twist vectors for the constraints + fuse_core::Vector3d linear_vel_mean; + linear_vel_mean << transformed_message.twist.twist.linear.x, + transformed_message.twist.twist.linear.y, + transformed_message.twist.twist.linear.z; + + // Create the covariance for the constraint + // TODO check if this the correct way for not doing copies + Eigen::Map cov_map( + transformed_message.twist.covariance.data()) ; + fuse_core::Matrix3d linear_vel_covariance = cov_map.block<3, 3>(0, 0); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), + linear_vel_mean_partial.rows()); + + populatePartialMeasurement( + linear_vel_mean, + linear_vel_covariance, + linear_indices, + linear_vel_mean_partial, + linear_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto linear_vel_constraint = + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, + linear_indices); + + linear_vel_constraint->loss(linear_velocity_loss); + + transaction.addVariable(velocity_linear); + transaction.addConstraint(linear_vel_constraint); + constraints_added = true; + } + } + + if (!angular_indices.empty()) { + // Create the twist variables + auto velocity_angular = + fuse_variables::VelocityAngular3DStamped::make_shared(twist.header.stamp, device_id); + velocity_angular->roll() = transformed_message.twist.twist.angular.x; + velocity_angular->pitch() = transformed_message.twist.twist.angular.y; + velocity_angular->yaw() = transformed_message.twist.twist.angular.z; + + fuse_core::Vector3d angular_vel_mean; + angular_vel_mean << transformed_message.twist.twist.angular.x, + transformed_message.twist.twist.angular.y, + transformed_message.twist.twist.angular.z; + + // Create the covariance for the constraint + // TODO check if this the correct way for not doing copies + Eigen::Map cov_map(transformed_message.twist.covariance.data()); + fuse_core::Matrix3d angular_vel_covariance = cov_map.block<3,3>(3,3); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); + fuse_core::MatrixXd angular_vel_covariance_partial(angular_vel_mean_partial.rows(), + angular_vel_mean_partial.rows()); + + populatePartialMeasurement( + angular_vel_mean, + angular_vel_covariance, + angular_indices, + angular_vel_mean_partial, + angular_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(angular_vel_mean_partial, angular_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Invalid partial angular velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto angular_vel_constraint = + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + source, *velocity_angular, angular_vel_mean_partial, angular_vel_covariance_partial, angular_indices); + + angular_vel_constraint->loss(angular_velocity_loss); + + transaction.addVariable(velocity_angular); + transaction.addConstraint(angular_vel_constraint); + constraints_added = true; + } + } + + if (constraints_added) { + transaction.addInvolvedStamp(twist.header.stamp); + } + + return constraints_added; +} + /** * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to * a fuse Transaction @@ -1297,27 +1852,6 @@ inline void scaleProcessNoiseCovariance( process_noise_covariance.topLeftCorner<6, 6>() = velocity * process_noise_covariance.topLeftCorner<6, 6>() * velocity.transpose(); } - -inline void scaleProcessNoiseCovariance( - fuse_core::Matrix16d & process_noise_covariance, - const fuse_core::Vector3d & velocity_linear, - const fuse_core::Vector3d & velocity_angular, - const double velocity_linear_norm_min, - const double velocity_angular_norm_min) -{ - fuse_core::Matrix3d velocity; - velocity.setIdentity(); - velocity.topLeftCorner<3, 3>().diagonal() *= - std::max( - velocity_linear_norm_min, - velocity_linear.norm()); - // velocity.bottomRightCorner<3, 3>().diagonal() *= - // std::max( - // velocity_angular_norm_min, - // velocity_angular.norm()); - process_noise_covariance.topLeftCorner<3, 3>() = - velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); -} } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index b5a77de98..6ff7b965a 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -35,10 +35,15 @@ #define FUSE_MODELS__COMMON__VARIABLE_TRAITS_HPP_ #include +#include #include +#include #include +#include #include +#include #include +#include namespace fuse_models @@ -71,6 +76,30 @@ struct is_linear_2d static const bool value = true; }; +template +struct is_linear_3d +{ + static const bool value = false; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + template struct is_angular_2d { @@ -89,6 +118,27 @@ struct is_angular_2d static const bool value = true; }; +template +struct is_angular_3d +{ + static const bool value = false; +}; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; +template +struct is_angular_vel_3d +{ + static const bool value = false; +}; +template<> +struct is_angular_vel_3d +{ + static const bool value = true; +}; + } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp new file mode 100644 index 000000000..26ffbbd7d --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -0,0 +1,185 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__ODOMETRY_3D_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces pose (relative or absolute) and velocity constraints + * from sensor data published by another node + * + * This sensor subscribes to a nav_msgs::msg::Odometry topic and: + * 1. Creates relative or absolute pose variables and constraints. If the \p differential parameter + * is set to false (the default), the measurement will be treated as an absolute constraint. If + * it is set to true, consecutive measurements will be used to generate relative pose + * constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the pose and twist components of the message, and processes + * them just like the Pose3D and Twist3D classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse measurements absolutely, or to + * create relative pose constraints using consecutive + * measurements. + * - pose_target_frame (string) Pose data will be transformed into this frame before it is fused. + * This frame should be a world-fixed frame, typically 'odom' or + * 'map'. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. This frame should be a body-relative frame, typically + * 'base_link'. + * + * Subscribes: + * - \p topic (nav_msgs::msg::Odometry) Odometry information at a given timestep + */ +class Odometry3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Odometry3D) + using ParameterType = parameters::Odometry3DParams; + + /** + * @brief Default constructor + */ + Odometry3D(); + + /** + * @brief Destructor + */ + virtual ~Odometry3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The pose message to process + */ + void process(const nav_msgs::msg::Odometry & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; + OdometryThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_HPP_ diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp new file mode 100644 index 000000000..913e9a511 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @class Odometry3DPublisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts + * a tf transform for optimized 3D state data (combination of Position3DStamped, + * Orientation3DStamped, VelocityLinear3DStamped, VelocityAngular3DStamped, and + * AccelerationLinear3DStamped). + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - publish_tf (bool, default: true) Whether to publish the generated pose data as a transform to + * the tf tree + * - predict_to_current_time (bool, default: false) The tf publication happens at a fixed rate. + * This parameter specifies whether we should + * predict, using the 3D unicycle model, the state + * at the time of the tf publication, rather than + * the last posterior (optimized) state. + * - publish_frequency (double, default: 10.0) How often, in Hz, we publish the filtered state + * data and broadcast the transform + * - tf_cache_time (double, default: 10.0) The length of our tf cache (only used if the + * world_frame_id and the map_frame_id are the same) + * - tf_timeout (double, default: 0.1) Our tf lookup timeout period (only used if the + * world_frame_id and the map_frame_id are the same) + * - queue_size (int, default: 1) The size of our ROS publication queue + * - map_frame_id (string, default: "map") Our map frame_id + * - odom_frame_id (string, default: "odom") Our odom frame_id + * - base_link_frame_id (string, default: "base_link") Our base_link (body) frame_id + * - world_frame_id (string, default: "odom") The frame_id that will be published as the parent + * frame for the output. Must be either the + * map_frame_id or the odom_frame_id. + * - topic (string, default: "odometry/filtered") The ROS topic to which we will publish the + * filtered state data + * + * Publishes: + * - odometry/filtered (nav_msgs::msg::Odometry) The most recent optimized state, gives as an + * odometry message + * - tf (via a tf2_ros::TransformBroadcaster) The most recent optimized state, as a tf transform + * + * Subscribes: + * - tf, tf_static (tf2_msgs::msg::TFMessage) Subscribes to tf data to obtain the requisite + * odom->base_link transform, but only if the + * world_frame_id is set to the value of the + * map_frame_id. + */ +class Odometry3DPublisher : public fuse_core::AsyncPublisher +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Odometry3DPublisher) + using ParameterType = parameters::Odometry3DPublisherParams; + + /** + * @brief Constructor + */ + Odometry3DPublisher(); + + /** + * @brief Destructor + */ + virtual ~Odometry3DPublisher() = default; + + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; + +protected: + /** + * @brief Perform any required post-construction initialization, such as advertising publishers or + * reading from the parameter server. + */ + void onInit() override; + + /** + * @brief Fires whenever an optimized graph has been computed + * + * @param[in] transaction A Transaction object, describing the set of variables that have been + * added and/or removed + * @param[in] graph A read-only pointer to the graph object, allowing queries to be + * performed whenever needed + */ + void notifyCallback( + fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required operations before the first call to notify() occurs + */ + void onStart() override; + + /** + * @brief Perform any required operations to stop publications + */ + void onStop() override; + + /** + * @brief Retrieves the given variable values at the requested time from the graph + * @param[in] graph The graph from which we will retrieve the state + * @param[in] stamp The time stamp at which we want the state + * @param[in] device_id The device ID for which we want the given variables + * @param[out] position_uuid The UUID of the position variable that gets extracted from the graph + * @param[out] orientation_uuid The UUID of the orientation variable that gets extracted from the + * graph + * @param[out] velocity_linear_uuid The UUID of the linear velocity variable that gets extracted + * from the graph + * @param[out] velocity_angular_uuid The UUID of the angular velocity variable that gets extracted + * from the graph + * @param[out] acceleration_linear_uuid The UUID of the linear acceleration variable that gets + * extracted from the graph + * @param[out] odometry All of the fuse pose and velocity variable values get packed into this + * structure + * @param[out] acceleration All of the fuse acceleration variable values get packed into this + * structure + * @return true if the checks pass, false otherwise + */ + bool getState( + const fuse_core::Graph & graph, + const rclcpp::Time & stamp, + const fuse_core::UUID & device_id, + fuse_core::UUID & position_uuid, + fuse_core::UUID & orientation_uuid, + fuse_core::UUID & velocity_linear_uuid, + fuse_core::UUID & velocity_angular_uuid, + fuse_core::UUID & acceleration_linear_uuid, + nav_msgs::msg::Odometry & odometry, + geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + + /** + * @brief Timer callback method for the filtered state publication and tf broadcasting + * @param[in] event The timer event parameters that are associated with the given invocation + */ + void publishTimerCallback(); + + /** + * @brief Object that searches for the most recent common timestamp for a set of variables + */ + using Synchronizer = fuse_publishers::StampedVariableSynchronizer< + fuse_variables::Orientation3DStamped, + fuse_variables::Position3DStamped, + fuse_variables::VelocityLinear3DStamped, + fuse_variables::VelocityAngular3DStamped, + fuse_variables::AccelerationLinear3DStamped>; + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The publisher's logger + + ParameterType params_; + + rclcpp::Time latest_stamp_; + rclcpp::Time latest_covariance_stamp_; + bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + nav_msgs::msg::Odometry odom_output_; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; + + //!< Object that tracks the latest common timestamp of multiple variables + Synchronizer synchronizer_; + + std::unique_ptr tf_buffer_; + + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr acceleration_pub_; + + std::shared_ptr tf_broadcaster_ = nullptr; + std::unique_ptr tf_listener_; + + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start + + rclcpp::TimerBase::SharedPtr publish_timer_; + + std::mutex mutex_; //!< A mutex to protect the access to the attributes used concurrently by the + //!< notifyCallback and publishTimerCallback methods: + //!< latest_stamp_, latest_covariance_stamp_, odom_output_ and + //!< acceleration_output_ +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp new file mode 100644 index 000000000..6d6c6d95f --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3D class + */ +struct Odometry3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + position_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "position_dimensions")); + orientation_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + linear_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_dimensions")); + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + pose_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "pose_target_frame"), + pose_target_frame); + + if (differential) { + // This can be avoided since we are calculating relative pose covariance with + // covariance_geometry + // independent = fuse_core::getParam( + // interfaces, fuse_core::joinParameterName( + // ns, + // "independent"), + // independent); + // use_twist_covariance = + // fuse_core::getParam( + // interfaces, fuse_core::joinParameterName( + // ns, + // "use_twist_covariance"), + // use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + // twist_covariance_offset = + // fuse_core::getCovarianceDiagonalParam<3>( + // interfaces, + // fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + linear_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + // fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + std::string topic {}; + std::string pose_target_frame {}; + std::string twist_target_frame {}; + std::vector position_indices; + std::vector orientation_indices; + std::vector linear_velocity_indices; + std::vector angular_velocity_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr linear_velocity_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp new file mode 100644 index 000000000..64dc5b485 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -0,0 +1,243 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3DPublisher class + */ +struct Odometry3DPublisherParams : public ParameterBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_tf"), + publish_tf); + invert_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "invert_tf"), + invert_tf); + predict_to_current_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_to_current_time"), + predict_to_current_time); + predict_with_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_with_acceleration"), + predict_with_acceleration); + publish_frequency = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_frequency"), + publish_frequency); + + process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>( + interfaces, fuse_core::joinParameterName( + ns, + "process_noise_diagonal"), 0.0); + scale_process_noise = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "scale_process_noise"), + scale_process_noise); + velocity_norm_min = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_norm_min"), + velocity_norm_min); + + fuse_core::getPositiveParam( + interfaces, + fuse_core::joinParameterName( + ns, + "covariance_throttle_period"), covariance_throttle_period, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_cache_time"), tf_cache_time, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + + map_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "map_frame_id"), + map_frame_id); + odom_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "odom_frame_id"), + odom_frame_id); + base_link_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_frame_id"), + base_link_frame_id); + base_link_output_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_output_frame_id"), + base_link_output_frame_id); + world_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "world_frame_id"), + world_frame_id); + + const bool frames_valid = + map_frame_id != odom_frame_id && + map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && + odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) { + RCLCPP_FATAL_STREAM( + interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + + assert(frames_valid); + } + + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + acceleration_topic = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_topic"), + acceleration_topic); + + fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); + } + + bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time {false}; + bool predict_with_acceleration {false}; + double publish_frequency {10.0}; + fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{false}; + double velocity_norm_min{1e-3}; + rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time {10, 0}; + rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + int queue_size {1}; + std::string map_frame_id {"map"}; + std::string odom_frame_id {"odom"}; + std::string base_link_frame_id {"base_link"}; + std::string base_link_output_frame_id {base_link_frame_id}; + std::string world_frame_id {odom_frame_id}; + std::string topic {"odometry/filtered"}; + std::string acceleration_topic {"acceleration/filtered"}; + ceres::Covariance::Options covariance_options; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp new file mode 100644 index 000000000..4405616f5 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp @@ -0,0 +1,218 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Unicycle3DIgnition class + */ +struct Unicycle3DIgnitionParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_on_startup = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_on_startup"), + publish_on_startup); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + reset_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "reset_service"), + reset_service); + set_pose_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_service"), + set_pose_service); + set_pose_deprecated_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_deprecated_service"), + set_pose_deprecated_service); + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + std::vector sigma_vector; + sigma_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_sigma"), + sigma_vector); + if (!sigma_vector.empty()) { + if (sigma_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must be length 15, but " + "is actually length " + + std::to_string(sigma_vector.size())); + } + auto is_sigma_valid = [](const double sigma) + { + return std::isfinite(sigma) && (sigma > 0); + }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); + } + initial_sigma.swap(sigma_vector); + } + + std::vector state_vector; + state_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_state"), + state_vector); + if (!state_vector.empty()) { + if (state_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_state parameter must be length 15, but is actually length " + + std::to_string(state_vector.size())); + } + auto is_state_valid = [](const double state) + { + return std::isfinite(state); + }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { + throw std::invalid_argument( + "The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); + } + initial_state.swap(state_vector); + } + + loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); + } + + + /** + * @brief Flag indicating if an initial state transaction should be sent on startup, or only in + * response to a set_pose service call or topic message. + */ + bool publish_on_startup {true}; + + /** + * @brief The size of the subscriber queue for the set_pose topic + */ + int queue_size {10}; + + /** + * @brief The name of the reset service to call before sending transactions to the optimizer + */ + std::string reset_service {"~/reset"}; + + /** + * @brief The name of the set_pose service to advertise + */ + std::string set_pose_service {"set_pose"}; + + /** + * @brief The name of the deprecated set_pose service without return codes + */ + std::string set_pose_deprecated_service {"set_pose_deprecated"}; + + /** + * @brief The topic name for received PoseWithCovarianceStamped messages + */ + std::string topic {"set_pose"}; + + /** + * @brief The uncertainty of the initial state value + * + * Standard deviations are provided as an 15-dimensional vector in the order: + * (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + * The covariance matrix is created placing the squared standard deviations along the diagonal of + * an 15x15 matrix. + */ + std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 + }; + + /** + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + */ + std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0}; + /** + * @brief Loss function + */ + fuse_core::Loss::SharedPtr loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/unicycle_3d.hpp index 050aacb13..7d12e30cc 100644 --- a/fuse_models/include/fuse_models/unicycle_3d.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d.hpp @@ -109,12 +109,12 @@ class Unicycle3D : public fuse_core::AsyncMotionModel fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration //!< variable - fuse_core::Vector3d position; //!< Map-frame position + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position // fuse_core::Vector3d orientation; //!< Map-frame orientation (roll, pitch, yaw) - fuse_core::Quaternion orientation; //!< Map-frame orientation (quaternion) - fuse_core::Vector3d vel_linear; //!< Body-frame linear velocities - fuse_core::Vector3d vel_angular; //!< Body-frame angular velocities - fuse_core::Vector3d acc_linear; //!< Body-frame linear (angular not needed) accelerations + fuse_core::Quaternion orientation = {1.0, 0.0, 0.0, 0.0}; //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations void print(std::ostream & stream = std::cout) const; @@ -207,7 +207,7 @@ class Unicycle3D : public fuse_core::AsyncMotionModel */ static void validateMotionModel( const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix16d & process_noise_covariance); + const fuse_core::Matrix15d & process_noise_covariance); fuse_core::node_interfaces::NodeInterfaces< fuse_core::node_interfaces::Base, @@ -225,7 +225,7 @@ class Unicycle3D : public fuse_core::AsyncMotionModel fuse_core::UUID device_id_; //!< The UUID of the device to be published fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created //!< motion model segments - fuse_core::Matrix16d process_noise_covariance_; //!< Process noise covariance matrix + fuse_core::Matrix15d process_noise_covariance_; //!< Process noise covariance matrix bool scale_process_noise_{false}; //!< Whether to scale the process noise //!< covariance pose by the norm of the current //!< state twist diff --git a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp index 81d21b07d..16080852a 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include @@ -58,12 +58,12 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * unicycle 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, and y_acc). When the sensor is first loaded, it publishes a single transaction + * unicycle 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this * ignition sensor resets the optimizer then publishes a new transaction with a prior at the - * specified pose. Priors on (x_vel, y_vel, yaw_vel, x_acc, and y_acc) continue to use the values + * specified pose. Priors on velocities and accelerations continue to use the values * configured on the parameter server. * * Parameters: @@ -78,7 +78,7 @@ namespace fuse_models * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for * the state. Variable order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index 5c968f4ff..844c1eb9c 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -43,397 +43,296 @@ namespace fuse_models { + using Jet = ceres::Jet; +// /** +// * @brief Given a state and time delta, predicts a new state - +// * inner templated version for ceres autodiff +// * @param[in] position1 - First position +// * @param[in] orientation1 - First orientation +// * @param[in] vel_linear1 - First linear velocity +// * @param[in] vel_angular1 - First angular velocity +// * @param[in] acc_linear1 - First linear acceleration +// * @param[in] dt - The time delta across which to predict the state +// * @param[out] position2 - Second position +// * @param[out] orientation2 - Second orientation +// * @param[out] vel_linear2 - Second linear velocity +// * @param[out] vel_angular2 - Second angular velocity +// * @param[out] acc_linear2 - Second linear acceleration +// */ +// template +// inline void predict_eigen( +// // const Eigen::Ref>& position1, +// // const Eigen::Ref>& orientation1, +// // const Eigen::Ref>& vel_linear1, +// // const Eigen::Ref>& vel_angular1, +// // const Eigen::Ref>& acc_linear1, +// // const T dt, +// // Eigen::Ref> position2, +// // Eigen::Ref> orientation2, +// // Eigen::Ref> vel_linear2, +// // Eigen::Ref> vel_angular2, +// // Eigen::Ref> acc_linear2) +// const Eigen::Matrix& position1, +// const Eigen::Quaternion& orientation1, +// const Eigen::Matrix& vel_linear1, +// const Eigen::Matrix& vel_angular1, +// const Eigen::Matrix& acc_linear1, +// const T dt, +// Eigen::Matrix position2, +// Eigen::Quaternion orientation2, +// Eigen::Matrix vel_linear2, +// Eigen::Matrix vel_angular2, +// Eigen::Matrix acc_linear2) +// { +// // Convert quaternion to eigen +// Eigen::Vector rpy( +// fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), +// fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), +// fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) +// ); +// // 3D material point projection model which matches the one used by r_l. +// T sr = ceres::sin(rpy.x()); +// T cr = ceres::cos(rpy.x()); +// T sp = ceres::sin(rpy.y()); +// T cp = ceres::cos(rpy.y()); +// T sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model +// T cy = ceres::cos(rpy.z()); +// T cpi = 1.0 / cp; +// T tp = ceres::tan(rpy.y()); + +// Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; +// Eigen::Matrix state_tf; + +// tf_pos(0, 0) = (cy * cp); +// tf_pos(0, 1) = (cy * sp * sr - sy * cr); +// tf_pos(0, 2) = (cy * sp * cr + sy * sr); +// tf_pos(1, 0) = (sy * cp); +// tf_pos(1, 1) = (sy * sp * sr + cy * cr); +// tf_pos(1, 2) = (sy * sp * cr - cy * sr); +// tf_pos(2, 0) = (-sp); +// tf_pos(2, 1) = (cp * sr); +// tf_pos(2, 2) = (cp * cr); + +// tf_rot(0, 0) = 1; +// tf_rot(0, 1) = sr * sp * cpi; +// tf_rot(0, 2) = cr * sp * cpi; +// tf_rot(1, 0) = T(0); +// tf_rot(1, 1) = cr; +// tf_rot(1, 2) = -sr; +// tf_rot(2, 0) = T(0); +// tf_rot(2, 1) = sr * cpi; +// tf_rot(2, 2) = cr * cpi; + +// // Project the state +// position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; +// rpy = rpy + tf_rot * vel_angular1 * dt; +// vel_linear2 = vel_linear1 + acc_linear1 * dt; +// vel_angular2 = vel_angular1; +// acc_linear2 = acc_linear1; + +// fuse_core::wrapAngle2D(rpy.x()); +// fuse_core::wrapAngle2D(rpy.y()); +// fuse_core::wrapAngle2D(rpy.z()); + +// // Convert back to quaternion +// orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * +// Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * +// Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); +// } /** - * @brief Given a state and time delta, predicts a new state + computes the Jacobians - * @param[in] position - First position - * @param[in] orientation - First orientation - * @param[in] vel_linear - First linear velocity - * @param[in] vel_angular - First angular velocity - * @param[in] acc_linear - First linear acceleration + * @brief Given a state and time delta, predicts a new state - + * inner templated version for ceres autodiff + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration * @param[in] dt - The time delta across which to predict the state - * @param[out] position_out - Second position - * @param[out] orientation_out - Second orientation - * @param[out] vel_linear_out - Second linear velocity - * @param[out] vel_angular_out - Second angular velocity - * @param[out] acc_linear_out - Second linear acceleration - * @param[out] state_tf_jacobian - Jacobian of the motion model + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration */ -template -inline void predict( - const Eigen::Matrix & position, - const Eigen::Quaternion & orientation, - const Eigen::Matrix & vel_linear, - const Eigen::Matrix & vel_angular, - const Eigen::Matrix & acc_linear, - const T dt, - Eigen::Matrix & position_out, - Eigen::Quaternion & orientation_out, - Eigen::Matrix & vel_linear_out, - Eigen::Matrix & vel_angular_out, - Eigen::Matrix & acc_linear_out, - Eigen::Matrix* state_tf_jacobian = nullptr) +inline void predict_eigen( + const Eigen::Matrix& position1, + const Eigen::Quaternion& orientation1, + const Eigen::Matrix& vel_linear1, + const Eigen::Matrix& vel_angular1, + const Eigen::Matrix& acc_linear1, + const double dt, + Eigen::Matrix position2, + Eigen::Quaternion orientation2, + Eigen::Matrix vel_linear2, + Eigen::Matrix vel_angular2, + Eigen::Matrix acc_linear2) { // Convert quaternion to eigen - T roll = fuse_core::getRoll(orientation.w(), orientation.x(), orientation.y(), orientation.z()); - T pitch = fuse_core::getPitch(orientation.w(), orientation.x(), orientation.y(), orientation.z()); - T yaw = fuse_core::getYaw(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + Eigen::Vector rpy( + fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) + ); + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); // 3D material point projection model which matches the one used by r_l. - T sr = ceres::sin(roll); - T cr = ceres::cos(roll); - T sp = ceres::sin(pitch); - T cp = ceres::cos(pitch); - T sy = ceres::sin(yaw); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - T cy = ceres::cos(yaw); - T cpi = 1.0 / cp; - T tp = ceres::tan(pitch); - - Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; - Eigen::Matrix state_tf; - - tf_pos(0, 0) = (cy * cp) * dt; - tf_pos(0, 1) = (cy * sp * sr - sy * cr) * dt; - tf_pos(0, 2) = (cy * sp * cr + sy * sr) * dt; - tf_pos(1, 0) = (sy * cp) * dt; - tf_pos(1, 1) = (sy * sp * sr + cy * cr) * dt; - tf_pos(1, 2) = (sy * sp * cr - cy * sr) * dt; - tf_pos(2, 0) = (-sp) * dt; - tf_pos(2, 1) = (cp * sr) * dt; - tf_pos(2, 2) = (cp * cr) * dt; - - tf_rot(0, 0) = dt; - tf_rot(0, 1) = sr * sp * cpi * dt; - tf_rot(0, 2) = cr * sp * cpi * dt; - tf_rot(1, 0) = T(0); - tf_rot(1, 1) = cr * dt; - tf_rot(1, 2) = -sr * dt; - tf_rot(2, 0) = T(0); - tf_rot(2, 1) = sr * cpi * dt; - tf_rot(2, 2) = cr * cpi * dt; - - // Compute the transfer function matrix - state_tf.setZero(); - // position - // state_tf.block(0, 0) = Eigen::Matrix::Identity(); - // state_tf.block<3, 3>(0, 6) = tf_pos; - // state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; - // // orientation - // state_tf.block<3, 3>(3, 3) = Eigen::Matrix::Identity(); - // state_tf.block<3, 3>(3, 9) = tf_rot; - // // linear velocity - // state_tf.block<3, 3>(6, 6) = Eigen::Matrix::Identity(); - // state_tf.block<3, 3>(6, 12) = dt * Eigen::Matrix::Identity(); - // // angular velocity - // state_tf.block<3, 3>(9, 9) = Eigen::Matrix::Identity(); - // // linear acceleration - // state_tf.block<3, 3>(12, 12) = Eigen::Matrix::Identity(); - - if (state_tf_jacobian) - { - // TODO: compute the jacobian of the motion model transfer function - T x_coeff = T(0.0); - T y_coeff = T(0.0); - T z_coeff = T(0.0); - T one_half_at_squared = 0.5 * dt * dt; - - y_coeff = cy * sp * cr + sy * sr; - z_coeff = -cy * sp * sr + sy * cr; - T dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - T dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; - - x_coeff = -cy * sp; - y_coeff = cy * cp * sr; - z_coeff = cy * cp * cr; - T dFx_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - T dFR_dP = - (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; - - x_coeff = -sy * cp; - y_coeff = -sy * sp * sr - cy * cr; - z_coeff = -sy * sp * cr + cy * sr; - T dFx_dY = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - y_coeff = sy * sp * cr - cy * sr; - z_coeff = -sy * sp * sr - cy * cr; - T dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - T dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; - - x_coeff = -sy * sp; - y_coeff = sy * cp * sr; - z_coeff = sy * cp * cr; - T dFy_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - x_coeff = cy * cp; - y_coeff = cy * sp * sr - sy * cr; - z_coeff = cy * sp * cr + sy * sr; - T dFy_dY = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - y_coeff = cp * cr; - z_coeff = -cp * sr; - T dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - T dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; - - x_coeff = -cp; - y_coeff = -sp * sr; - z_coeff = -sp * cr; - T dFz_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - T dFY_dP = - (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; - - *state_tf_jacobian = state_tf; - (*state_tf_jacobian)(0, 3) = dFx_dR; - (*state_tf_jacobian)(0, 4) = dFx_dP; - (*state_tf_jacobian)(0, 5) = dFx_dY; - (*state_tf_jacobian)(1, 3) = dFy_dR; - (*state_tf_jacobian)(1, 4) = dFy_dP; - (*state_tf_jacobian)(1, 5) = dFy_dY; - (*state_tf_jacobian)(2, 3) = dFz_dR; - (*state_tf_jacobian)(2, 4) = dFz_dP; - (*state_tf_jacobian)(3, 3) = dFR_dR; - (*state_tf_jacobian)(3, 4) = dFR_dP; - (*state_tf_jacobian)(4, 3) = dFP_dR; - (*state_tf_jacobian)(5, 3) = dFY_dR; - (*state_tf_jacobian)(5, 4) = dFY_dP; - } - - // Predict the new state - Eigen::Matrix state; - state.head(3) = position; - state.segment(3,3) = Eigen::Matrix(roll, pitch, yaw); - state.segment(6,3) = vel_linear; - state.segment(9,3) = vel_angular; - state.tail(3) = acc_linear; - - state = state_tf * state; - - // Convert back - position_out = state.head(3); - fuse_core::wrapAngle2D(state(5)); - fuse_core::wrapAngle2D(state(4)); - fuse_core::wrapAngle2D(state(3)); - Eigen::Quaterniond q = - Eigen::AngleAxis(state(5), Eigen::Matrix::UnitZ()) * - Eigen::AngleAxis(state(4), Eigen::Matrix::UnitY()) * - Eigen::AngleAxis(state(3), Eigen::Matrix::UnitX()); - orientation_out = q; - vel_linear_out = state.segment(6,3); - vel_angular_out = state.segment(9,3); - acc_linear_out = state.tail(3); + double sr = ceres::sin(rpy.x()); + double cr = ceres::cos(rpy.x()); + double sp = ceres::sin(rpy.y()); + double cp = ceres::cos(rpy.y()); + double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + double cy = ceres::cos(rpy.z()); + double cpi = 1.0 / cp; + + Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; + Eigen::Matrix state_tf; + + tf_pos(0, 0) = (cy * cp); // vx1 to x2 + tf_pos(0, 1) = (cy * sp * sr - sy * cr); // vy1 to x2 + tf_pos(0, 2) = (cy * sp * cr + sy * sr); // vz1 to x2 + tf_pos(1, 0) = (sy * cp); // vx1 to y2 + tf_pos(1, 1) = (sy * sp * sr + cy * cr); // vy1 to y2 + tf_pos(1, 2) = (sy * sp * cr - cy * sr); // vz1 to y2 + tf_pos(2, 0) = (-sp); // vx1 to z2 + tf_pos(2, 1) = (cp * sr); // vy1 to z2 + tf_pos(2, 2) = (cp * cr); // vz1 to z2 + + tf_rot(0, 0) = 1; + tf_rot(0, 1) = sr * sp * cpi; + tf_rot(0, 2) = cr * sp * cpi; + tf_rot(1, 0) = double(0); + tf_rot(1, 1) = cr; + tf_rot(1, 2) = -sr; + tf_rot(2, 0) = double(0); + tf_rot(2, 1) = sr * cpi; + tf_rot(2, 2) = cr * cpi; + + // Project the state + position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; + rpy = rpy + tf_rot * vel_angular1 * dt; + vel_linear2 = vel_linear1 + acc_linear1 * dt; + vel_angular2 = vel_angular1; + acc_linear2 = acc_linear1; + + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + + // Convert back to quaternion + orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); } /** - * @brief Given a state and time delta, predicts a new state + computes the Jacobians - * @param[in] position - First position - * @param[in] orientation - First orientation - * @param[in] vel_linear - First linear velocity - * @param[in] vel_angular - First angular velocity - * @param[in] acc_linear - First linear acceleration + * @brief Given a state and time delta, predicts a new state - + * inner templated version for ceres autodiff + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration * @param[in] dt - The time delta across which to predict the state - * @param[out] position_out - Second position - * @param[out] orientation_out - Second orientation - * @param[out] vel_linear_out - Second linear velocity - * @param[out] vel_angular_out - Second angular velocity - * @param[out] acc_linear_out - Second linear acceleration - * @param[out] state_tf_jacobian - Jacobian of the motion model + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration */ -inline void predict( - const fuse_core::Vector3d & position, - // const fuse_core::Vector3d & orientation, - const fuse_core::Quaternion & orientation, - const fuse_core::Vector3d & vel_linear, - const fuse_core::Vector3d & vel_angular, - const fuse_core::Vector3d & acc_linear, - const double dt, - fuse_core::Vector3d & position_out, - // fuse_core::Vector3d & orientation_out, - fuse_core::Quaternion & orientation_out, - fuse_core::Vector3d & vel_linear_out, - fuse_core::Vector3d & vel_angular_out, - fuse_core::Vector3d & acc_linear_out, - fuse_core::Matrix15d* state_tf_jacobian = nullptr) +inline void predict_eigen( + const Eigen::Matrix& position1, + const Eigen::Quaternion& orientation1, + const Eigen::Matrix& vel_linear1, + const Eigen::Matrix& vel_angular1, + const Eigen::Matrix& acc_linear1, + const Jet dt, + Eigen::Matrix position2, + Eigen::Quaternion orientation2, + Eigen::Matrix vel_linear2, + Eigen::Matrix vel_angular2, + Eigen::Matrix acc_linear2) { // Convert quaternion to eigen - double roll = fuse_core::getRoll(orientation.w(), orientation.x(), orientation.y(), orientation.z()); - double pitch = fuse_core::getPitch(orientation.w(), orientation.x(), orientation.y(), orientation.z()); - double yaw = fuse_core::getYaw(orientation.w(), orientation.x(), orientation.y(), orientation.z()); + Eigen::Vector rpy( + fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) + ); + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); // 3D material point projection model which matches the one used by r_l. - double sr = ceres::sin(roll); - double cr = ceres::cos(roll); - double sp = ceres::sin(pitch); - double cp = ceres::cos(pitch); - double sy = ceres::sin(yaw); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - double cy = ceres::cos(yaw); - double cpi = 1.0 / cp; - double tp = ceres::tan(pitch); - - fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; - fuse_core::Matrix15d state_tf; - - tf_pos(0, 0) = (cy * cp) * dt; - tf_pos(0, 1) = (cy * sp * sr - sy * cr) * dt; - tf_pos(0, 2) = (cy * sp * cr + sy * sr) * dt; - tf_pos(1, 0) = (sy * cp) * dt; - tf_pos(1, 1) = (sy * sp * sr + cy * cr) * dt; - tf_pos(1, 2) = (sy * sp * cr - cy * sr) * dt; - tf_pos(2, 0) = (-sp) * dt; - tf_pos(2, 1) = (cp * sr) * dt; - tf_pos(2, 2) = (cp * cr) * dt; - - tf_rot(0, 0) = dt; - tf_rot(0, 1) = sr * sp * cpi * dt; - tf_rot(0, 2) = cr * sp * cpi * dt; - tf_rot(1, 0) = 0; - tf_rot(1, 1) = cr * dt; - tf_rot(1, 2) = -sr * dt; - tf_rot(2, 0) = 0; - tf_rot(2, 1) = sr * cpi * dt; - tf_rot(2, 2) = cr * cpi * dt; - - // Compute the transfer function matrix - state_tf.setZero(); - // position - state_tf.block<3, 3>(0, 0) = fuse_core::Matrix3d::Identity(); - state_tf.block<3, 3>(0, 6) = tf_pos; - state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; - // orientation - state_tf.block<3, 3>(3, 3) = fuse_core::Matrix3d::Identity(); - state_tf.block<3, 3>(3, 9) = tf_rot; - // linear velocity - state_tf.block<3, 3>(6, 6) = fuse_core::Matrix3d::Identity(); - state_tf.block<3, 3>(6, 12) = dt * fuse_core::Matrix3d::Identity(); - // angular velocity - state_tf.block<3, 3>(9, 9) = fuse_core::Matrix3d::Identity(); - // linear acceleration - state_tf.block<3, 3>(12, 12) = fuse_core::Matrix3d::Identity(); - - if (state_tf_jacobian) - { - // TODO: compute the jacobian of the motion model transfer function - double x_coeff = 0.0; - double y_coeff = 0.0; - double z_coeff = 0.0; - double one_half_at_squared = 0.5 * dt * dt; - - y_coeff = cy * sp * cr + sy * sr; - z_coeff = -cy * sp * sr + sy * cr; - double dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - double dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; - - x_coeff = -cy * sp; - y_coeff = cy * cp * sr; - z_coeff = cy * cp * cr; - double dFx_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - double dFR_dP = - (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; - - x_coeff = -sy * cp; - y_coeff = -sy * sp * sr - cy * cr; - z_coeff = -sy * sp * cr + cy * sr; - double dFx_dY = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - y_coeff = sy * sp * cr - cy * sr; - z_coeff = -sy * sp * sr - cy * cr; - double dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - double dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; - - x_coeff = -sy * sp; - y_coeff = sy * cp * sr; - z_coeff = sy * cp * cr; - double dFy_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - x_coeff = cy * cp; - y_coeff = cy * sp * sr - sy * cr; - z_coeff = cy * sp * cr + sy * sr; - double dFy_dY = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - - y_coeff = cp * cr; - z_coeff = -cp * sr; - double dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - double dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; - - x_coeff = -cp; - y_coeff = -sp * sr; - z_coeff = -sp * cr; - double dFz_dP = - (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - one_half_at_squared; - double dFY_dP = - (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; - - *state_tf_jacobian = state_tf; - (*state_tf_jacobian)(0, 3) = dFx_dR; - (*state_tf_jacobian)(0, 4) = dFx_dP; - (*state_tf_jacobian)(0, 5) = dFx_dY; - (*state_tf_jacobian)(1, 3) = dFy_dR; - (*state_tf_jacobian)(1, 4) = dFy_dP; - (*state_tf_jacobian)(1, 5) = dFy_dY; - (*state_tf_jacobian)(2, 3) = dFz_dR; - (*state_tf_jacobian)(2, 4) = dFz_dP; - (*state_tf_jacobian)(3, 3) = dFR_dR; - (*state_tf_jacobian)(3, 4) = dFR_dP; - (*state_tf_jacobian)(4, 3) = dFP_dR; - (*state_tf_jacobian)(5, 3) = dFY_dR; - (*state_tf_jacobian)(5, 4) = dFY_dP; - } - - // Predict the new state - fuse_core::Vector15d state; - state.head(3) = position; - state.segment(3,3) = fuse_core::Vector3d(roll, pitch, yaw); - state.segment(6,3) = vel_linear; - state.segment(9,3) = vel_angular; - state.tail(3) = acc_linear; - - state = state_tf * state; - - // Convert back - position_out = state.head(3); - fuse_core::wrapAngle2D(state(5)); - fuse_core::wrapAngle2D(state(4)); - fuse_core::wrapAngle2D(state(3)); - Eigen::Quaterniond q = - Eigen::AngleAxisd(state(5), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(state(4), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(state(3), Eigen::Vector3d::UnitX()); - orientation_out = q; - vel_linear_out = state.segment(6,3); - vel_angular_out = state.segment(9,3); - acc_linear_out = state.tail(3); + Jet sr = ceres::sin(rpy.x()); + Jet cr = ceres::cos(rpy.x()); + Jet sp = ceres::sin(rpy.y()); + Jet cp = ceres::cos(rpy.y()); + Jet sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + Jet cy = ceres::cos(rpy.z()); + Jet cpi = 1.0 / cp; + + Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; + Eigen::Matrix state_tf; + + tf_pos(0, 0) = (cy * cp); + tf_pos(0, 1) = (cy * sp * sr - sy * cr); + tf_pos(0, 2) = (cy * sp * cr + sy * sr); + tf_pos(1, 0) = (sy * cp); + tf_pos(1, 1) = (sy * sp * sr + cy * cr); + tf_pos(1, 2) = (sy * sp * cr - cy * sr); + tf_pos(2, 0) = (-sp); + tf_pos(2, 1) = (cp * sr); + tf_pos(2, 2) = (cp * cr); + + tf_rot(0, 0) = Jet(1); + tf_rot(0, 1) = sr * sp * cpi; + tf_rot(0, 2) = cr * sp * cpi; + tf_rot(1, 0) = Jet(0); + tf_rot(1, 1) = cr; + tf_rot(1, 2) = -sr; + tf_rot(2, 0) = Jet(0); + tf_rot(2, 1) = sr * cpi; + tf_rot(2, 2) = cr * cpi; + + // Project the state + position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; + rpy = rpy + tf_rot * vel_angular1 * dt; + vel_linear2 = vel_linear1 + acc_linear1 * dt; + vel_angular2 = vel_angular1; + acc_linear2 = acc_linear1; + + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + + // Convert back to quaternion + orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); + + // Project the state - dummy versions + position2 = position1; + orientation2 = orientation1; + vel_linear2 = vel_linear1; + vel_angular2 = vel_angular1; + acc_linear2 = acc_linear1; } +/** + * @brief Given a state and time delta, predicts a new state - + * Templated version to be called from ceres autodiff costfunction + * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) + * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) + */ template inline void predict( const T * const position1, @@ -448,190 +347,258 @@ inline void predict( T * const vel_angular2, T * const acc_linear2) { - // Creates some Eigen maps and pass them to the predict function - // First test just to try if this works, create new eigen vector and pass these to the predict function - Eigen::Matrix position_in(position1[0], position1[1], position1[2]); - Eigen::Quaternion orientation_in(orientation1[3], orientation1[0], orientation1[1], orientation1[2]); - Eigen::Matrix vel_linear_in(vel_linear1[0], vel_linear1[1], vel_linear1[2]); - Eigen::Matrix vel_angular_in(vel_angular1[0], vel_angular1[1], vel_angular1[2]); - Eigen::Matrix acc_linear_in(acc_linear1[0], acc_linear1[1], acc_linear1[2]); - Eigen::Matrix position_out, vel_linear_out, vel_angular_out, acc_linear_out; - Eigen::Quaternion orientation_out; - predict( - position_in, - orientation_in, - vel_linear_in, - vel_angular_in, - acc_linear_in, + // conversion from array to eigen + // TODO: check how to map quaternion for rowmajor arrays + const Eigen::Matrix position1_eigen = + Eigen::Map> (position1); + const Eigen::Quaternion orientation1_eigen = + Eigen::Map> (orientation1); + const Eigen::Matrix vel_linear1_eigen = + Eigen::Map> (vel_linear1); + const Eigen::Matrix vel_angular1_eigen = + Eigen::Map> (vel_angular1); + const Eigen::Matrix acc_linear1_eigen = + Eigen::Map> (acc_linear1); + + Eigen::Matrix position2_eigen = + Eigen::Map> (position2); + Eigen::Quaternion orientation2_eigen = + Eigen::Map> (orientation2); + Eigen::Matrix vel_linear2_eigen = + Eigen::Map> (vel_linear2); + Eigen::Matrix vel_angular2_eigen = + Eigen::Map> (vel_angular2); + Eigen::Matrix acc_linear2_eigen = + Eigen::Map> (acc_linear2); + + predict_eigen( + position1_eigen, + orientation1_eigen, + vel_linear1_eigen, + vel_angular1_eigen, + acc_linear1_eigen, dt, - position_out, - orientation_out, - vel_linear_out, - vel_angular_out, - acc_linear_out); - - // Convert back - position2[0] = position_out.x(); - position2[1] = position_out.y(); - position2[2] = position_out.z(); - orientation2[0] = orientation_out.x(); - orientation2[1] = orientation_out.y(); - orientation2[2] = orientation_out.z(); - orientation2[3] = orientation_out.w(); - vel_linear2[0] = vel_linear_out.x(); - vel_linear2[1] = vel_linear_out.y(); - vel_linear2[2] = vel_linear_out.z(); - vel_angular2[0] = vel_angular_out.x(); - vel_angular2[1] = vel_angular_out.y(); - vel_angular2[2] = vel_angular_out.z(); - acc_linear2[0] = acc_linear_out.x(); - acc_linear2[1] = acc_linear_out.y(); - acc_linear2[2] = acc_linear_out.z(); + position2_eigen, + orientation2_eigen, + vel_linear2_eigen, + vel_angular2_eigen, + acc_linear2_eigen); } +/** + * @brief Given a state and time delta, predicts a new state + computes the Jacobians - + * version for predicting new states inside the plugin + * @param[in] position - First position + * @param[in] orientation - First orientation + * @param[in] vel_linear - First linear velocity + * @param[in] vel_angular - First angular velocity + * @param[in] acc_linear - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position_out - Second position + * @param[out] orientation_out - Second orientation + * @param[out] vel_linear_out - Second linear velocity + * @param[out] vel_angular_out - Second angular velocity + * @param[out] acc_linear_out - Second linear acceleration + * @param[out] state_tf_jacobian - Jacobian of the motion model + */ +inline void predict( + const fuse_core::Vector3d & position1, + const fuse_core::Quaternion & orientation1, + const fuse_core::Vector3d & vel_linear1, + const fuse_core::Vector3d & vel_angular1, + const fuse_core::Vector3d & acc_linear1, + const double dt, + fuse_core::Vector3d & position2, + fuse_core::Quaternion & orientation2, + fuse_core::Vector3d & vel_linear2, + fuse_core::Vector3d & vel_angular2, + fuse_core::Vector3d & acc_linear2) +{ + // This is all double code - keep it like this if we want to use also analytic diff, otherwise we + // can call the templated version above from here + // Convert quaternion to eigen + fuse_core::Vector3d rpy( + fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) + ); + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + // 3D material point projection model which matches the one used by r_l. + double sr = ceres::sin(rpy.x()); + double cr = ceres::cos(rpy.x()); + double sp = ceres::sin(rpy.y()); + double cp = ceres::cos(rpy.y()); + double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + double cy = ceres::cos(rpy.z()); + double cpi = 1.0 / cp; -// /** -// * @brief Given a state and time delta, predicts a new state -// * @param[in] position1_x - First X position -// * @param[in] position1_y - First Y position -// * @param[in] position1_z - First Z position -// * @param[in] roll1 - First roll -// * @param[in] pitch1 - First pitch -// * @param[in] yaw1 - First yaw -// * @param[in] vel_linear1_x - First X velocity -// * @param[in] vel_linear1_y - First Y velocity -// * @param[in] vel_linear1_z - First Z velocity -// * @param[in] vel_roll1 - First roll velocity -// * @param[in] vel_pitch1 - First pitch velocity -// * @param[in] vel_yaw1 - First yaw velocity -// * @param[in] acc_linear1_x - First X acceleration -// * @param[in] acc_linear1_y - First Y acceleration -// * @param[in] acc_linear1_z - First Z acceleration -// * @param[in] dt - The time delta across which to predict the state -// * @param[out] position2_x - Second X position -// * @param[out] position2_y - Second Y position -// * @param[out] position2_z - Second Z position -// * @param[out] roll2 - Second roll -// * @param[out] pitch2 - Second pitch -// * @param[out] yaw2 - Second orientation -// * @param[out] vel_linear2_x - Second X velocity -// * @param[out] vel_linear2_y - Second Y velocity -// * @param[out] vel_linear2_z - Second Z velocity -// * @param[out] vel_roll2 - Second roll velocity -// * @param[out] vel_pitch2 - Second pitch velocity -// * @param[out] vel_yaw2 - Second yaw velocity -// * @param[out] acc_linear2_x - Second X acceleration -// * @param[out] acc_linear2_y - Second Y acceleration -// * @param[out] acc_linear2_z - Second Z acceleration -// * @param[out] jacobians - Jacobians wrt the state -// */ -// inline void predict( -// const double position1_x, -// const double position1_y, -// const double position1_z, -// // const double roll1, -// // const double pitch1, -// // const double yaw1, -// const double q1_x, -// const double q1_y, -// const double q1_z, -// const double q1_w, -// const double vel_linear1_x, -// const double vel_linear1_y, -// const double vel_linear1_z, -// const double vel_roll1, -// const double vel_pitch1, -// const double vel_yaw1, -// const double acc_linear1_x, -// const double acc_linear1_y, -// const double acc_linear1_z, -// const double dt, -// double & position2_x, -// double & position2_y, -// double & position2_z, -// // double & roll2, -// // double & pitch2, -// // double & yaw2, -// const double q2_x, -// const double q2_y, -// const double q2_z, -// const double q2_w, -// double & vel_linear2_x, -// double & vel_linear2_y, -// double & vel_linear2_z, -// double & vel_roll2, -// double & vel_pitch2, -// double & vel_yaw2, -// double & acc_linear2_x, -// double & acc_linear2_y, -// double & acc_linear2_z, -// double ** jacobians) -// { -// fuse_core::Vector3d position1(position1_x, position1_y, position1_z); -// // fuse_core::Vector3d orientation1(roll1, pitch1, yaw1); -// fuse_core::Quaternion orientation1(q1_w, q1_x, q1_y, q1_z); -// fuse_core::Vector3d vel_linear1(vel_linear1_x, vel_linear1_y, vel_linear1_z); -// fuse_core::Vector3d vel_angular1(vel_roll1, vel_pitch1, vel_yaw1); -// fuse_core::Vector3d acc_linear1(acc_linear1_x, acc_linear1_y, acc_linear1_z); -// fuse_core::Vector3d position2, orientation2, vel_linear2, vel_angular2, acc_linear2; -// fuse_core::Quaternion orientation2; -// fuse_core::Matrix15d* jacobian; - -// predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, -// position2, orientation2, vel_linear2, vel_angular2, acc_linear2, jacobian); + fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; + fuse_core::Matrix15d state_tf; + + tf_pos(0, 0) = (cy * cp); + tf_pos(0, 1) = (cy * sp * sr - sy * cr); + tf_pos(0, 2) = (cy * sp * cr + sy * sr); + tf_pos(1, 0) = (sy * cp); + tf_pos(1, 1) = (sy * sp * sr + cy * cr); + tf_pos(1, 2) = (sy * sp * cr - cy * sr); + tf_pos(2, 0) = (-sp); + tf_pos(2, 1) = (cp * sr); + tf_pos(2, 2) = (cp * cr); + + tf_rot(0, 0) = 1.0; + tf_rot(0, 1) = sr * sp * cpi; + tf_rot(0, 2) = cr * sp * cpi; + tf_rot(1, 0) = 0.0; + tf_rot(1, 1) = cr; + tf_rot(1, 2) = -sr; + tf_rot(2, 0) = 0.0; + tf_rot(2, 1) = sr * cpi; + tf_rot(2, 2) = cr * cpi; + + // Project the state + position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; + rpy = rpy + tf_rot * vel_angular1 * dt; + vel_linear2 = vel_linear1 + acc_linear1 * dt; + vel_angular2 = vel_angular1; + acc_linear2 = acc_linear1; + + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + + // Convert back to quaternion + orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + + // // Compute the transfer function matrix + // state_tf.setZero(); + // // position + // state_tf.block<3, 3>(0, 0) = fuse_core::Matrix3d::Identity(); + // state_tf.block<3, 3>(0, 6) = tf_pos; + // state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; + // // orientation + // state_tf.block<3, 3>(3, 3) = fuse_core::Matrix3d::Identity(); + // state_tf.block<3, 3>(3, 9) = tf_rot; + // // linear velocity + // state_tf.block<3, 3>(6, 6) = fuse_core::Matrix3d::Identity(); + // state_tf.block<3, 3>(6, 12) = dt * fuse_core::Matrix3d::Identity(); + // // angular velocity + // state_tf.block<3, 3>(9, 9) = fuse_core::Matrix3d::Identity(); + // // linear acceleration + // state_tf.block<3, 3>(12, 12) = fuse_core::Matrix3d::Identity(); + + // if (state_tf_jacobian) + // { + // // TODO: compute the jacobian of the motion model transfer function + // double x_coeff = 0.0; + // double y_coeff = 0.0; + // double z_coeff = 0.0; + // double one_half_at_squared = 0.5 * dt * dt; + + // y_coeff = cy * sp * cr + sy * sr; + // z_coeff = -cy * sp * sr + sy * cr; + // double dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + // double dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; + + // x_coeff = -cy * sp; + // y_coeff = cy * cp * sr; + // z_coeff = cy * cp * cr; + // double dFx_dP = + // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + // one_half_at_squared; + // double dFR_dP = + // (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; + + // x_coeff = -sy * cp; + // y_coeff = -sy * sp * sr - cy * cr; + // z_coeff = -sy * sp * cr + cy * sr; + // double dFx_dY = + // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + // one_half_at_squared; + + // y_coeff = sy * sp * cr - cy * sr; + // z_coeff = -sy * sp * sr - cy * cr; + // double dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + // double dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; + + // x_coeff = -sy * sp; + // y_coeff = sy * cp * sr; + // z_coeff = sy * cp * cr; + // double dFy_dP = + // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + // one_half_at_squared; + + // x_coeff = cy * cp; + // y_coeff = cy * sp * sr - sy * cr; + // z_coeff = cy * sp * cr + sy * sr; + // double dFy_dY = + // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + // one_half_at_squared; + + // y_coeff = cp * cr; + // z_coeff = -cp * sr; + // double dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; + // double dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; + + // x_coeff = -cp; + // y_coeff = -sp * sr; + // z_coeff = -sp * cr; + // double dFz_dP = + // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + + // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * + // one_half_at_squared; + // double dFY_dP = + // (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; + + // *state_tf_jacobian = state_tf; + // (*state_tf_jacobian)(0, 3) = dFx_dR; + // (*state_tf_jacobian)(0, 4) = dFx_dP; + // (*state_tf_jacobian)(0, 5) = dFx_dY; + // (*state_tf_jacobian)(1, 3) = dFy_dR; + // (*state_tf_jacobian)(1, 4) = dFy_dP; + // (*state_tf_jacobian)(1, 5) = dFy_dY; + // (*state_tf_jacobian)(2, 3) = dFz_dR; + // (*state_tf_jacobian)(2, 4) = dFz_dP; + // (*state_tf_jacobian)(3, 3) = dFR_dR; + // (*state_tf_jacobian)(3, 4) = dFR_dP; + // (*state_tf_jacobian)(4, 3) = dFP_dR; + // (*state_tf_jacobian)(5, 3) = dFY_dR; + // (*state_tf_jacobian)(5, 4) = dFY_dP; + // } + + // // Predict the new state + // fuse_core::Vector15d state; + // state.head(3) = position; + // state.segment(3,3) = fuse_core::Vector3d(roll, pitch, yaw); + // state.segment(6,3) = vel_linear; + // state.segment(9,3) = vel_angular; + // state.tail(3) = acc_linear; -// position2_x = position2.x(); -// position2_y = position2.y(); -// position2_z = position2.z(); -// // roll2 = orientation2.x(); -// // pitch2 = orientation2.y(); -// // yaw2 = orientation2.z(); -// q2_x = orientation2.x(); -// q2_y = orientation2.y(); -// q2_z = orientation2.z(); -// q2_w = orientation2.w(); -// vel_linear2_x = vel_linear2.x(); -// vel_linear2_y = vel_linear2.y(); -// vel_linear2_z = vel_linear2.z(); -// vel_roll2 = vel_angular2.x(); -// vel_pitch2 = vel_angular2.y(); -// vel_yaw2 = vel_angular2.z(); -// acc_linear2_x = acc_linear2.x(); -// acc_linear2_y = acc_linear2.y(); -// acc_linear2_z = acc_linear2.z(); - -// if (jacobians) -// { -// // Jacobian wrt position1 -// if (jacobians[0]) { -// Eigen::Map> j_temp(jacobians[0]); -// j_temp = jacobian->block<15, 3>(0, 0); -// } -// // Jacobian wrt orientation1 -// // if (jacobians[1]) { -// // Eigen::Map> j_temp(jacobians[1]); -// // j_temp = jacobian->block<15, 3>(0, 3); -// // } -// if (jacobians[1]) { -// Eigen::Map> j_temp(jacobians[1]); -// j_temp = jacobian->block<15, 3>(0, 3); -// } -// // Jacobian wrt vel_linear1 -// if (jacobians[2]) { -// Eigen::Map> j_temp(jacobians[2]); -// j_temp = jacobian->block<15, 3>(0, 6); -// } -// // Jacobian wrt vel_angular1 -// if (jacobians[3]) { -// Eigen::Map> j_temp(jacobians[3]); -// j_temp = jacobian->block<15, 3>(0, 9); -// } -// // Jacobian wrt acc_linear1 -// if (jacobians[4]) { -// Eigen::Map> j_temp(jacobians[4]); -// j_temp = jacobian->block<15, 3>(0, 12); -// } -// } -// } + // state = state_tf * state; + + // // Convert back + // position_out = state.head(3); + // fuse_core::wrapAngle2D(state(5)); + // fuse_core::wrapAngle2D(state(4)); + // fuse_core::wrapAngle2D(state(3)); + // Eigen::Quaterniond q = + // Eigen::AngleAxisd(state(5), Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(state(4), Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(state(3), Eigen::Vector3d::UnitX()); + // orientation_out = q; + // vel_linear_out = state.segment(6,3); + // vel_angular_out = state.segment(9,3); + // acc_linear_out = state.tail(3); +} } // namespace fuse_models #endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp new file mode 100644 index 000000000..22b71a0bf --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp @@ -0,0 +1,311 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ + +#include +#include +#include + +#include +#include +#include "covariance_geometry/pose_representation.hpp" + +namespace fuse_models +{ + using Jet = ceres::Jet; + +template +inline void predict( + const T position1_x, + const T position1_y, + const T position1_z, + const T quat1_w, + const T quat1_x, + const T quat1_y, + const T quat1_z, + const T vel_linear1_x, + const T vel_linear1_y, + const T vel_linear1_z, + const T vel_roll1, + const T vel_pitch1, + const T vel_yaw1, + const T acc_linear1_x, + const T acc_linear1_y, + const T acc_linear1_z, + const T dt, + T & position2_x, + T & position2_y, + T & position2_z, + T & quat2_w, + T & quat2_x, + T & quat2_y, + T & quat2_z, + T & vel_linear2_x, + T & vel_linear2_y, + T & vel_linear2_z, + T & vel_roll2, + T & vel_pitch2, + T & vel_yaw2, + T & acc_linear2_x, + T & acc_linear2_y, + T & acc_linear2_z) +{ + // Convert quaternion to eigen + T roll1 = fuse_core::getRoll(quat1_w, quat1_x, quat1_y, quat1_z); + T pitch1 = fuse_core::getPitch(quat1_w, quat1_x, quat1_y, quat1_z); + T yaw1 = fuse_core::getYaw(quat1_w, quat1_x, quat1_y, quat1_z); + fuse_core::wrapAngle2D(roll1); + fuse_core::wrapAngle2D(pitch1); + fuse_core::wrapAngle2D(yaw1); + // 3D material point projection model which matches the one used by r_l. + T sr = ceres::sin(roll1); + T cr = ceres::cos(roll1); + T sp = ceres::sin(pitch1); + T cp = ceres::cos(pitch1); + T sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + T cy = ceres::cos(yaw1); + T cpi = 1.0 / cp; + + // Project the state + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + + 0.5 * ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt * dt; + + position2_y = position1_y + + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + + 0.5 * ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt * dt; + + position2_z = position1_z + + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + 0.5 * ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt * dt; + + T roll2 = roll1 + + (vel_roll1 + sr * sp * cpi * vel_pitch1 + cr * sp * cpi * vel_yaw1) * dt; + T pitch2 = pitch1 + + (T(0) * vel_roll1 + cr * vel_pitch1 - sr * vel_yaw1) * dt; + T yaw2 = yaw1 + + (T(0) * vel_roll1 + sr * cpi * vel_pitch1 + cr * cpi * vel_yaw1) * dt; + + fuse_core::wrapAngle2D(roll2); + fuse_core::wrapAngle2D(pitch2); + fuse_core::wrapAngle2D(yaw2); + + Eigen::Quaternion q; + q.w() = cos(roll2 / T(2)) * cos(pitch2 / T(2)) * cos(yaw2 / T(2)) + + sin(roll2 / T(2)) * sin(pitch2 / T(2)) * sin(yaw2 / T(2)); + q.x() = sin(roll2 / T(2)) * cos(pitch2 / T(2)) * cos(yaw2 / T(2)) - + cos(roll2 / T(2)) * sin(pitch2 / T(2)) * sin(yaw2 / T(2)); + q.y() = cos(roll2 / T(2)) * sin(pitch2 / T(2)) * cos(yaw2 / T(2)) + + sin(roll2 / T(2)) * cos(pitch2 / T(2)) * sin(yaw2 / T(2)); + q.z() = cos(roll2 / T(2)) * cos(pitch2 / T(2)) * sin(yaw2 / T(2)) - + sin(roll2 / T(2)) * sin(pitch2 / T(2)) * cos(yaw2 / T(2)); + q.normalize(); + + quat2_w = q.w(); + quat2_x = q.x(); + quat2_y = q.y(); + quat2_z = q.z(); + + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + + vel_roll2 = vel_roll1; + vel_pitch2 = vel_pitch1; + vel_yaw2 = vel_yaw1; + + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position (array with x at index 0, y at index 1) + * @param[in] yaw1 - First orientation + * @param[in] vel_linear1 - First velocity (array with x at index 0, y at index 1) + * @param[in] vel_yaw1 - First yaw velocity + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1) + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position (array with x at index 0, y at index 1) + * @param[out] yaw2 - Second orientation + * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1) + * @param[out] vel_yaw2 - Second yaw velocity + * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) + */ +template +inline void predict( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T dt, + T * const position2, + T * const orientation2, + T * const vel_linear2, + T * const vel_angular2, + T * const acc_linear2) +{ + predict( + position1[0], + position1[1], + position1[2], + orientation1[0], + orientation1[1], + orientation1[2], + orientation1[3], + vel_linear1[0], + vel_linear1[1], + vel_linear1[2], + vel_angular1[0], + vel_angular1[1], + vel_angular1[2], + acc_linear1[0], + acc_linear1[1], + acc_linear1[2], + dt, + position2[0], + position2[1], + position2[2], + orientation2[0], + orientation2[1], + orientation2[2], + orientation2[3], + vel_linear2[0], + vel_linear2[1], + vel_linear2[2], + vel_angular2[0], + vel_angular2[1], + vel_angular2[2], + acc_linear2[0], + acc_linear2[1], + acc_linear2[2]); +} + +/** + * @brief Given a state and time delta, predicts a new state + computes the Jacobians - + * version for predicting new states inside the plugin + * @param[in] position - First position + * @param[in] orientation - First orientation + * @param[in] vel_linear - First linear velocity + * @param[in] vel_angular - First angular velocity + * @param[in] acc_linear - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position_out - Second position + * @param[out] orientation_out - Second orientation + * @param[out] vel_linear_out - Second linear velocity + * @param[out] vel_angular_out - Second angular velocity + * @param[out] acc_linear_out - Second linear acceleration + * @param[out] state_tf_jacobian - Jacobian of the motion model + */ +inline void predict( + const fuse_core::Vector3d & position1, + const fuse_core::Quaternion & orientation1, + const fuse_core::Vector3d & vel_linear1, + const fuse_core::Vector3d & vel_angular1, + const fuse_core::Vector3d & acc_linear1, + const double dt, + fuse_core::Vector3d & position2, + fuse_core::Quaternion & orientation2, + fuse_core::Vector3d & vel_linear2, + fuse_core::Vector3d & vel_angular2, + fuse_core::Vector3d & acc_linear2) +{ + // This is all double code - keep it like this if we want to use also analytic diff, otherwise we + // can call the templated version above from here + // Convert quaternion to eigen + fuse_core::Vector3d rpy( + fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) + ); + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + // 3D material point projection model which matches the one used by r_l. + double sr = ceres::sin(rpy.x()); + double cr = ceres::cos(rpy.x()); + double sp = ceres::sin(rpy.y()); + double cp = ceres::cos(rpy.y()); + double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + double cy = ceres::cos(rpy.z()); + double cpi = 1.0 / cp; + + fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; + fuse_core::Matrix15d state_tf; + + tf_pos(0, 0) = (cy * cp); + tf_pos(0, 1) = (cy * sp * sr - sy * cr); + tf_pos(0, 2) = (cy * sp * cr + sy * sr); + tf_pos(1, 0) = (sy * cp); + tf_pos(1, 1) = (sy * sp * sr + cy * cr); + tf_pos(1, 2) = (sy * sp * cr - cy * sr); + tf_pos(2, 0) = (-sp); + tf_pos(2, 1) = (cp * sr); + tf_pos(2, 2) = (cp * cr); + + tf_rot(0, 0) = 1.0; + tf_rot(0, 1) = sr * sp * cpi; + tf_rot(0, 2) = cr * sp * cpi; + tf_rot(1, 0) = 0.0; + tf_rot(1, 1) = cr; + tf_rot(1, 2) = -sr; + tf_rot(2, 0) = 0.0; + tf_rot(2, 1) = sr * cpi; + tf_rot(2, 2) = cr * cpi; + + // Project the state + position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; + rpy = rpy + tf_rot * vel_angular1 * dt; + vel_linear2 = vel_linear1 + acc_linear1 * dt; + vel_angular2 = vel_angular1; + acc_linear2 = acc_linear1; + + fuse_core::wrapAngle2D(rpy.x()); + fuse_core::wrapAngle2D(rpy.y()); + fuse_core::wrapAngle2D(rpy.z()); + + // Convert back to quaternion + orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + + std::cout << "Predicted position: " << position2.transpose() << std::endl; + std::cout << "Predicted orientation: w: " << orientation2.w() << " x:" << orientation2.x() << " y:" << orientation2.y() << " z:" << orientation2.z() << std::endl; +} +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index 5a9fee5c1..f467685ba 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -34,6 +34,7 @@ #ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ #define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +// #include #include #include @@ -49,7 +50,7 @@ namespace fuse_models * * The state vector includes the following quantities, given in this order: * pose translation x, y, z - * pose orientation (in quaternion) x, y, z, w + * pose orientation (in quaternion) w, x, y, z * linear velocity x, y, z * angular velocity x, y, z * linear acceleration x, y, z @@ -89,21 +90,21 @@ class Unicycle3DStateCostFunctor * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) */ - Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix16d & A); + Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. - * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) - * @param[in] orientation1 - First orientation (array with x at index 0, y at index 1, z at index 2, w at index 3) - * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) - * @param[in] vel_angular1 - First angular velocity (array with x at index 0, y at index 1, z at index 2) - * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) - * @param[in] position2 - Second position (array with x at index 0, y at index 1, z at index 2) - * @param[in] orientation2 - Second orientation (array with x at index 0, y at index 1, z at index 2, w at index 3) - * @param[in] vel_linear2 - Second linear velocity (array with x at index 0, y at index 1, z at index 2) - * @param[in] vel_angular2 - Second angular velocity (array with x at index 0, y at index 1, z at index 2) - * @param[in] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) - * @param[out] residual - The computed residual (error) + * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) + * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ template bool operator()( @@ -121,13 +122,13 @@ class Unicycle3DStateCostFunctor private: double dt_; - fuse_core::Matrix16d A_; //!< The residual weighting matrix, most likely the square root + fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix }; Unicycle3DStateCostFunctor::Unicycle3DStateCostFunctor( const double dt, - const fuse_core::Matrix16d & A) + const fuse_core::Matrix15d & A) : dt_(dt), A_(A) { @@ -147,11 +148,11 @@ bool Unicycle3DStateCostFunctor::operator()( const T * const acc_linear2, T * residual) const { - T position_pred[3]; - T orientation_pred[4]; - T vel_linear_pred[3]; - T vel_angular_pred[3]; - T acc_linear_pred[3]; + T position_pred[3] {T(0.0)}; + T orientation_pred[4] {T(0.0)}; + T vel_linear_pred[3] {T(0.0)}; + T vel_angular_pred[3] {T(0.0)}; + T acc_linear_pred[3] {T(0.0)}; predict( position1, orientation1, @@ -165,12 +166,9 @@ bool Unicycle3DStateCostFunctor::operator()( vel_angular_pred, acc_linear_pred); - Eigen::Map> residuals_map(residual); - Eigen::Map> q_pred(orientation_pred); - Eigen::Quaternion q2(orientation2[3], orientation2[0], orientation2[1], orientation2[2]); - Eigen::Quaternion q_res; - // q_pred(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); - q_res = q2.inverse() * q_pred; + Eigen::Map> residuals_map(residual); + Eigen::Map> q_pred(orientation_pred), q2(orientation2); + Eigen::Quaternion q_res = q2.inverse() * q_pred; residuals_map(0) = T(position2[0] - position_pred[0]); residuals_map(1) = T(position2[1] - position_pred[1]); @@ -178,19 +176,16 @@ bool Unicycle3DStateCostFunctor::operator()( residuals_map(3) = T(q_res.x()); residuals_map(4) = T(q_res.y()); residuals_map(5) = T(q_res.z()); - residuals_map(6) = T(q_res.w()); - residuals_map(7) = T(vel_linear2[0] - vel_linear_pred[0]); - residuals_map(8) = T(vel_linear2[1] - vel_linear_pred[1]); - residuals_map(9) = T(vel_linear2[2] - vel_linear_pred[2]); - residuals_map(10) = T(vel_angular2[0] - vel_angular_pred[0]); - residuals_map(11) = T(vel_angular2[1] - vel_angular_pred[1]); - residuals_map(12) = T(vel_angular2[2] - vel_angular_pred[2]); - residuals_map(13) = T(acc_linear2[0] - acc_linear_pred[0]); - residuals_map(14) = T(acc_linear2[1] - acc_linear_pred[1]); - residuals_map(15) = T(acc_linear2[2] - acc_linear_pred[2]); - - // This should be already done in the predict function - // fuse_core::wrapAngle2D(residuals_map(2)); + // residuals_map(6) = T(q_res.w()); + residuals_map(6) = T(vel_linear2[0] - vel_linear_pred[0]); + residuals_map(7) = T(vel_linear2[1] - vel_linear_pred[1]); + residuals_map(8) = T(vel_linear2[2] - vel_linear_pred[2]); + residuals_map(9) = T(vel_angular2[0] - vel_angular_pred[0]); + residuals_map(10) = T(vel_angular2[1] - vel_angular_pred[1]); + residuals_map(11) = T(vel_angular2[2] - vel_angular_pred[2]); + residuals_map(12) = T(acc_linear2[0] - acc_linear_pred[0]); + residuals_map(13) = T(acc_linear2[1] - acc_linear_pred[1]); + residuals_map(14) = T(acc_linear2[2] - acc_linear_pred[2]); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp index ee5b583c4..f2c306e4d 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp @@ -106,7 +106,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint const fuse_variables::VelocityLinear3DStamped & velocity_linear2, const fuse_variables::VelocityAngular3DStamped & velocity_angular2, const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix16d & covariance); + const fuse_core::Matrix15d & covariance); /** * @brief Destructor @@ -125,7 +125,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - const fuse_core::Matrix16d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix15d & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. @@ -133,7 +133,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - fuse_core::Matrix16d covariance() const + fuse_core::Matrix15d covariance() const { return (sqrt_information_.transpose() * sqrt_information_).inverse(); } @@ -159,7 +159,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint protected: double dt_; //!< The time delta for the constraint - fuse_core::Matrix16d sqrt_information_; //!< The square root information matrix + fuse_core::Matrix15d sqrt_information_; //!< The square root information matrix private: // Allow Boost Serialization access to private methods diff --git a/fuse_models/package.xml b/fuse_models/package.xml index ab880388d..6dfb9cd1c 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -17,6 +17,7 @@ libceres-dev eigen + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs @@ -35,6 +36,7 @@ tf2_geometry_msgs tf2_ros + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp new file mode 100644 index 000000000..7389c8c6f --- /dev/null +++ b/fuse_models/src/odometry_3d.cpp @@ -0,0 +1,230 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Odometry3D::Odometry3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) +{ +} + +void Odometry3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); + std::cout << "Odometry3D initialized" << std::endl; +} + +void Odometry3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.position_indices.empty() && params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Odometry3D::onStart() +{ + if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &OdometryThrottledCallback::callback< + const nav_msgs::msg::Odometry &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Odometry3D::onStop() +{ + sub_.reset(); +} + +void Odometry3D::process(const nav_msgs::msg::Odometry & msg) +{ + // Create a transaction object + std::cout << "Odometry3D process" << std::endl; + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the pose data + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose = msg.pose; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.header.frame_id = msg.child_frame_id; + twist.twist = msg.twist; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.pose_target_frame, + params_.position_indices, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + params_.linear_velocity_loss, + params_.angular_velocity_loss, + params_.twist_target_frame, + params_.linear_velocity_indices, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + transaction->print(std::cout); + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Odometry3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time( + pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp new file mode 100644 index 000000000..7d0a61333 --- /dev/null +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -0,0 +1,644 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #include "covariance_geometry_ros/covariance_geometry_ros.hpp" +// #include "covariance_geometry_ros/utils.hpp" + +// Register this publisher with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry3DPublisher, fuse_core::Publisher) + +namespace fuse_models +{ + +Odometry3DPublisher::Odometry3DPublisher() +: fuse_core::AsyncPublisher(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), + latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) +{ +} + +void Odometry3DPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces, name); +} + +void Odometry3DPublisher::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + tf_broadcaster_ = std::make_shared(interfaces_); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + tf_buffer_ = std::make_unique( + clock_, + params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ + ); + + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); + } + + // Advertise the topics + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + odom_pub_ = rclcpp::create_publisher( + interfaces_, + params_.topic, + params_.queue_size, + pub_options); + acceleration_pub_ = rclcpp::create_publisher( + interfaces_, + params_.acceleration_topic, + params_.queue_size, + pub_options); +} + +void Odometry3DPublisher::notifyCallback( + fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) +{ + // Find the most recent common timestamp + const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); + if (0u == latest_stamp.nanoseconds()) { + { + std::lock_guard lock(mutex_); + latest_stamp_ = latest_stamp; + } + + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" + << device_id_ << "'."); + return; + } + + // Get the pose values associated with the selected timestamp + fuse_core::UUID position_uuid; + fuse_core::UUID orientation_uuid; + fuse_core::UUID velocity_linear_uuid; + fuse_core::UUID velocity_angular_uuid; + fuse_core::UUID acceleration_linear_uuid; + + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; + if (!getState( + *graph, + latest_stamp, + device_id_, + position_uuid, + orientation_uuid, + velocity_linear_uuid, + velocity_angular_uuid, + acceleration_linear_uuid, + odom_output, + acceleration_output)) + { + std::lock_guard lock(mutex_); + latest_stamp_ = latest_stamp; + return; + } + + odom_output.header.frame_id = params_.world_frame_id; + odom_output.header.stamp = latest_stamp; + odom_output.child_frame_id = params_.base_link_output_frame_id; + + acceleration_output.header.frame_id = params_.base_link_output_frame_id; + acceleration_output.header.stamp = latest_stamp; + + // Don't waste CPU computing the covariance if nobody is listening + rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; + bool latest_covariance_valid = latest_covariance_valid_; + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + // Throttle covariance computation + if (params_.covariance_throttle_period.nanoseconds() == 0 || + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + { + latest_covariance_stamp = latest_stamp; + + try { + std::vector> covariance_requests; + covariance_requests.emplace_back(position_uuid, position_uuid); + covariance_requests.emplace_back(position_uuid, orientation_uuid); + covariance_requests.emplace_back(orientation_uuid, orientation_uuid); + covariance_requests.emplace_back(velocity_linear_uuid, velocity_linear_uuid); + covariance_requests.emplace_back(velocity_linear_uuid, velocity_angular_uuid); + covariance_requests.emplace_back(velocity_angular_uuid, velocity_angular_uuid); + covariance_requests.emplace_back(acceleration_linear_uuid, acceleration_linear_uuid); + + std::vector> covariance_matrices; + graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options); + + odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) + odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) + odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) + odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) + odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) + odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) + + odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) + odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) + odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) + odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) + odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) + odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) + + odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) + odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) + odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) + odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) + odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) + odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) + + odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) + odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) + odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) + odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) + odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) + odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) + + odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) + odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) + odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) + odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) + odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) + odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) + + odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) + odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) + odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) + odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) + odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) + odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) + + odom_output.twist.covariance[0] = covariance_matrices[3][0]; + odom_output.twist.covariance[1] = covariance_matrices[3][1]; + odom_output.twist.covariance[2] = covariance_matrices[3][2]; + odom_output.twist.covariance[3] = covariance_matrices[4][0]; + odom_output.twist.covariance[4] = covariance_matrices[4][1]; + odom_output.twist.covariance[5] = covariance_matrices[4][2]; + + odom_output.twist.covariance[6] = covariance_matrices[3][3]; + odom_output.twist.covariance[7] = covariance_matrices[3][4]; + odom_output.twist.covariance[8] = covariance_matrices[3][5]; + odom_output.twist.covariance[9] = covariance_matrices[4][3]; + odom_output.twist.covariance[10] = covariance_matrices[4][4]; + odom_output.twist.covariance[11] = covariance_matrices[4][5]; + + odom_output.twist.covariance[12] = covariance_matrices[3][6]; + odom_output.twist.covariance[13] = covariance_matrices[3][7]; + odom_output.twist.covariance[14] = covariance_matrices[3][8]; + odom_output.twist.covariance[15] = covariance_matrices[4][6]; + odom_output.twist.covariance[16] = covariance_matrices[4][7]; + odom_output.twist.covariance[17] = covariance_matrices[4][8]; + + odom_output.twist.covariance[18] = covariance_matrices[4][0]; + odom_output.twist.covariance[19] = covariance_matrices[4][3]; + odom_output.twist.covariance[20] = covariance_matrices[4][6]; + odom_output.twist.covariance[21] = covariance_matrices[5][0]; + odom_output.twist.covariance[22] = covariance_matrices[5][1]; + odom_output.twist.covariance[23] = covariance_matrices[5][2]; + + odom_output.twist.covariance[24] = covariance_matrices[4][1]; + odom_output.twist.covariance[25] = covariance_matrices[4][4]; + odom_output.twist.covariance[26] = covariance_matrices[4][7]; + odom_output.twist.covariance[27] = covariance_matrices[5][1]; + odom_output.twist.covariance[28] = covariance_matrices[5][4]; + odom_output.twist.covariance[29] = covariance_matrices[5][5]; + + odom_output.twist.covariance[30] = covariance_matrices[4][2]; + odom_output.twist.covariance[31] = covariance_matrices[4][5]; + odom_output.twist.covariance[32] = covariance_matrices[4][8]; + odom_output.twist.covariance[33] = covariance_matrices[5][2]; + odom_output.twist.covariance[34] = covariance_matrices[5][5]; + odom_output.twist.covariance[35] = covariance_matrices[5][8]; + + acceleration_output.accel.covariance[0] = covariance_matrices[6][0]; + acceleration_output.accel.covariance[1] = covariance_matrices[6][1]; + acceleration_output.accel.covariance[2] = covariance_matrices[6][2]; + acceleration_output.accel.covariance[6] = covariance_matrices[6][3]; + acceleration_output.accel.covariance[7] = covariance_matrices[6][4]; + acceleration_output.accel.covariance[8] = covariance_matrices[6][5]; + acceleration_output.accel.covariance[12] = covariance_matrices[6][6]; + acceleration_output.accel.covariance[13] = covariance_matrices[6][7]; + acceleration_output.accel.covariance[14] = covariance_matrices[6][8]; + + // test if covariances are symmetric + Eigen::Map odom_cov_map(odom_output.pose.covariance.data()); + if (!odom_cov_map.isApprox(odom_cov_map.transpose())) { + throw std::runtime_error("Odometry covariance matrix is not symmetric"); + } + + Eigen::Map twist_cov_map(odom_output.twist.covariance.data()); + if (!twist_cov_map.isApprox(twist_cov_map.transpose())) { + throw std::runtime_error("Twist covariance matrix is not symmetric"); + } + + Eigen::Map accel_cov_map(acceleration_output.accel.covariance.data()); + if (!accel_cov_map.isApprox(accel_cov_map.transpose())) { + throw std::runtime_error("Acceleration covariance matrix is not symmetric"); + } + + latest_covariance_valid = true; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + logger_, + "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() + << ". The covariance will be set to zero.\n" + << e.what()); + std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); + std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); + std::fill( + acceleration_output.accel.covariance.begin(), + acceleration_output.accel.covariance.end(), 0.0); + + latest_covariance_valid = false; + } + } else { + // This covariance computation cycle has been skipped, so simply take the last covariance + // computed + // + // We do not propagate the latest covariance forward because it would grow unbounded being + // very different from the actual covariance we would have computed if not throttling. + odom_output.pose.covariance = odom_output_.pose.covariance; + odom_output.twist.covariance = odom_output_.twist.covariance; + acceleration_output.accel.covariance = acceleration_output_.accel.covariance; + } + } + + { + std::lock_guard lock(mutex_); + + latest_stamp_ = latest_stamp; + latest_covariance_stamp_ = latest_covariance_stamp; + latest_covariance_valid_ = latest_covariance_valid; + odom_output_ = odom_output; + acceleration_output_ = acceleration_output; + } +} + +void Odometry3DPublisher::onStart() +{ + synchronizer_ = Synchronizer(device_id_); + latest_stamp_ = latest_covariance_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + latest_covariance_valid_ = false; + odom_output_ = nav_msgs::msg::Odometry(); + acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); + + // TODO(CH3): Add this to a separate callback group for async behavior + publish_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), + cb_group_ + ); + + delayed_throttle_filter_.reset(); +} + +void Odometry3DPublisher::onStop() +{ + publish_timer_->cancel(); +} + +bool Odometry3DPublisher::getState( + const fuse_core::Graph & graph, + const rclcpp::Time & stamp, + const fuse_core::UUID & device_id, + fuse_core::UUID & position_uuid, + fuse_core::UUID & orientation_uuid, + fuse_core::UUID & velocity_linear_uuid, + fuse_core::UUID & velocity_angular_uuid, + fuse_core::UUID & acceleration_linear_uuid, + nav_msgs::msg::Odometry & odometry, + geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +{ + try { + position_uuid = fuse_variables::Position3DStamped(stamp, device_id).uuid(); + auto position_variable = dynamic_cast( + graph.getVariable(position_uuid)); + + orientation_uuid = fuse_variables::Orientation3DStamped(stamp, device_id).uuid(); + auto orientation_variable = dynamic_cast( + graph.getVariable(orientation_uuid)); + + velocity_linear_uuid = fuse_variables::VelocityLinear3DStamped(stamp, device_id).uuid(); + auto velocity_linear_variable = dynamic_cast( + graph.getVariable(velocity_linear_uuid)); + + velocity_angular_uuid = fuse_variables::VelocityAngular3DStamped(stamp, device_id).uuid(); + auto velocity_angular_variable = dynamic_cast( + graph.getVariable(velocity_angular_uuid)); + + acceleration_linear_uuid = fuse_variables::AccelerationLinear3DStamped(stamp, device_id).uuid(); + auto acceleration_linear_variable = + dynamic_cast( + graph.getVariable(acceleration_linear_uuid)); + + odometry.pose.pose.position.x = position_variable.x(); + odometry.pose.pose.position.y = position_variable.y(); + odometry.pose.pose.position.z = position_variable.z(); + odometry.pose.pose.orientation.w = orientation_variable.w(); + odometry.pose.pose.orientation.x = orientation_variable.x(); + odometry.pose.pose.orientation.y = orientation_variable.y(); + odometry.pose.pose.orientation.z = orientation_variable.z(); + odometry.twist.twist.linear.x = velocity_linear_variable.x(); + odometry.twist.twist.linear.y = velocity_linear_variable.y(); + odometry.twist.twist.linear.z = velocity_linear_variable.z(); + odometry.twist.twist.angular.x = velocity_angular_variable.roll(); + odometry.twist.twist.angular.y = velocity_angular_variable.pitch(); + odometry.twist.twist.angular.z = velocity_angular_variable.yaw(); + + acceleration.accel.accel.linear.x = acceleration_linear_variable.x(); + acceleration.accel.accel.linear.y = acceleration_linear_variable.y(); + acceleration.accel.accel.linear.z = acceleration_linear_variable.z(); + acceleration.accel.accel.angular.x = 0.0; + acceleration.accel.accel.angular.y = 0.0; + acceleration.accel.accel.angular.z = 0.0; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + return false; + } catch (...) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + return false; + } + + return true; +} + +void Odometry3DPublisher::publishTimerCallback() +{ + rclcpp::Time latest_stamp; + rclcpp::Time latest_covariance_stamp; + bool latest_covariance_valid; + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; + { + std::lock_guard lock(mutex_); + + latest_stamp = latest_stamp_; + latest_covariance_stamp = latest_covariance_stamp_; + latest_covariance_valid = latest_covariance_valid_; + odom_output = odom_output_; + acceleration_output = acceleration_output_; + } + + if (0u == latest_stamp.nanoseconds()) { + RCLCPP_WARN_STREAM_EXPRESSION( + logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); + return; + } + + tf2::Transform pose; + tf2::fromMsg(odom_output.pose.pose, pose); + // covariance_geometry::PoseQuaternionCovarianceRPY pose; + // covariance_geometry::fromROS(odom_output.pose, pose); + + // If requested, we need to project our state forward in time using the 3D kinematic model + if (params_.predict_to_current_time) { + // TODO: this is in pause since we need to implement the jacobian of the 3D predict function + // rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); + // fuse_core::Vector3d velocity_linear, velocity_angular; + // tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); + // tf2::fromMsg(odom_output.twist.twist.angular, velocity_angular); + + // const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + + // fuse_core::Matrix8d jacobian; // TODO: maybe this is not useful if we rely on covariance_geometry + + // tf2::Vector3 acceleration_linear; + // if (params_.predict_with_acceleration) { + // tf2::fromMsg(acceleration_output.accel.accel.linear, acceleration_linear); + // } + + // double yaw_vel; + + // predict( + // pose, + // velocity_linear, + // velocity_angular, + // acceleration_linear, + // dt, + // pose, + // velocity_linear, + // velocity_angular, + // acceleration_linear); + + // odom_output.pose.pose.position = tf2::toMsg(pose.getOrigin()); + // odom_output.pose.pose.orientation = tf2::toMsg(pose.getRotation()); + + // odom_output.twist.twist.linear.x = velocity_linear.x(); + // odom_output.twist.twist.linear.y = velocity_linear.y(); + // odom_output.twist.twist.linear.z = velocity_linear.z(); + // odom_output.twist.twist.angular.x = velocity_angular.x(); + // odom_output.twist.twist.angular.y = velocity_angular.y(); + // odom_output.twist.twist.angular.z = velocity_angular.z(); + + // if (params_.predict_with_acceleration) { + // acceleration_output.accel.accel.linear.x = acceleration_linear.x(); + // acceleration_output.accel.accel.linear.y = acceleration_linear.y(); + // acceleration_output.accel.accel.linear.z = acceleration_linear.z(); + // } + + // odom_output.header.stamp = timer_now; + // acceleration_output.header.stamp = timer_now; + + // // Either the last covariance computation was skipped because there was no subscriber, + // // or it failed + // if (latest_covariance_valid) { + // fuse_core::Matrix8d covariance; + // covariance(0, 0) = odom_output.pose.covariance[0]; + // covariance(0, 1) = odom_output.pose.covariance[1]; + // covariance(0, 2) = odom_output.pose.covariance[5]; + // covariance(1, 0) = odom_output.pose.covariance[6]; + // covariance(1, 1) = odom_output.pose.covariance[7]; + // covariance(1, 2) = odom_output.pose.covariance[11]; + // covariance(2, 0) = odom_output.pose.covariance[30]; + // covariance(2, 1) = odom_output.pose.covariance[31]; + // covariance(2, 2) = odom_output.pose.covariance[35]; + + // covariance(3, 3) = odom_output.twist.covariance[0]; + // covariance(3, 4) = odom_output.twist.covariance[1]; + // covariance(3, 5) = odom_output.twist.covariance[5]; + // covariance(4, 3) = odom_output.twist.covariance[6]; + // covariance(4, 4) = odom_output.twist.covariance[7]; + // covariance(4, 5) = odom_output.twist.covariance[11]; + // covariance(5, 3) = odom_output.twist.covariance[30]; + // covariance(5, 4) = odom_output.twist.covariance[31]; + // covariance(5, 5) = odom_output.twist.covariance[35]; + + // covariance(6, 6) = acceleration_output.accel.covariance[0]; + // covariance(6, 7) = acceleration_output.accel.covariance[1]; + // covariance(7, 6) = acceleration_output.accel.covariance[6]; + // covariance(7, 7) = acceleration_output.accel.covariance[7]; + + // // TODO(efernandez) for now we set to zero the out-of-diagonal blocks with the correlations + // // between pose, twist and acceleration, but we could cache them in another + // // attribute when we retrieve the covariance from the ceres problem + // covariance.topRightCorner<3, 5>().setZero(); + // covariance.bottomLeftCorner<5, 3>().setZero(); + // covariance.block<3, 2>(3, 6).setZero(); + // covariance.block<2, 3>(6, 3).setZero(); + + // covariance = jacobian * covariance * jacobian.transpose(); + + // auto process_noise_covariance = params_.process_noise_covariance; + // if (params_.scale_process_noise) { + // common::scaleProcessNoiseCovariance( + // process_noise_covariance, velocity_linear, + // odom_output.twist.twist.angular.z, params_.velocity_norm_min); + // } + + // covariance.noalias() += dt * process_noise_covariance; + + // odom_output.pose.covariance[0] = covariance(0, 0); + // odom_output.pose.covariance[1] = covariance(0, 1); + // odom_output.pose.covariance[5] = covariance(0, 2); + // odom_output.pose.covariance[6] = covariance(1, 0); + // odom_output.pose.covariance[7] = covariance(1, 1); + // odom_output.pose.covariance[11] = covariance(1, 2); + // odom_output.pose.covariance[30] = covariance(2, 0); + // odom_output.pose.covariance[31] = covariance(2, 1); + // odom_output.pose.covariance[35] = covariance(2, 2); + + // odom_output.twist.covariance[0] = covariance(3, 3); + // odom_output.twist.covariance[1] = covariance(3, 4); + // odom_output.twist.covariance[5] = covariance(3, 5); + // odom_output.twist.covariance[6] = covariance(4, 3); + // odom_output.twist.covariance[7] = covariance(4, 4); + // odom_output.twist.covariance[11] = covariance(4, 5); + // odom_output.twist.covariance[30] = covariance(5, 3); + // odom_output.twist.covariance[31] = covariance(5, 4); + // odom_output.twist.covariance[35] = covariance(5, 5); + + // acceleration_output.accel.covariance[0] = covariance(6, 6); + // acceleration_output.accel.covariance[1] = covariance(6, 7); + // acceleration_output.accel.covariance[6] = covariance(7, 6); + // acceleration_output.accel.covariance[7] = covariance(7, 7); + // } + } + + odom_pub_->publish(odom_output); + acceleration_pub_->publish(acceleration_output); + + if (params_.publish_tf) { + auto frame_id = odom_output.header.frame_id; + auto child_frame_id = odom_output.child_frame_id; + + if (params_.invert_tf) { + pose = pose.inverse(); + std::swap(frame_id, child_frame_id); + } + + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = odom_output.header.stamp; + trans.header.frame_id = frame_id; + trans.child_frame_id = child_frame_id; + trans.transform.translation = tf2::toMsg(pose.getOrigin()); + trans.transform.rotation = tf2::toMsg(pose.getRotation()); + + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + try { + auto base_to_odom = tf_buffer_->lookupTransform( + params_.base_link_frame_id, + params_.odom_frame_id, + trans.header.stamp, + params_.tf_timeout); + + geometry_msgs::msg::TransformStamped map_to_odom; + tf2::doTransform(base_to_odom, map_to_odom, trans); + map_to_odom.child_frame_id = params_.odom_frame_id; + trans = map_to_odom; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id << " transform. Error: " << e.what()); + + return; + } + } + + tf_broadcaster_->sendTransform(trans); + } +} + +} // namespace fuse_models diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 4b235a458..c6dba7484 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -174,8 +174,6 @@ bool Unicycle2D::applyCallback(fuse_core::Transaction & transaction) // manager, in turn, makes calls to the generateMotionModel() function. try { // Now actually generate the motion model segments - std::cout << "Transaction: " << std::endl; - transaction.print(std::cout); timestamp_manager_.query(transaction, true); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -311,8 +309,6 @@ void Unicycle2D::generateMotionModel( } else { state1 = base_state; } - // std::cout << "State1: " << state1 << std::endl; - state1.print(std::cout); // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); @@ -344,10 +340,7 @@ void Unicycle2D::generateMotionModel( state2.velocity_linear, state2.velocity_yaw, state2.acceleration_linear); - - // std::cout << "State2: " << state2 << std::endl; - state2.print(std::cout); - + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 9accc5061..3ad47a4ad 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -32,12 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include #include -#include +#include +// #include #include #include #include @@ -103,13 +105,13 @@ ceres::CostFunction * Unicycle2DStateKinematicConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // - // return new ceres::AutoDiffCostFunction( new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); + return new ceres::AutoDiffCostFunction( new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); // // which requires: // // #include - return new Unicycle2DStateCostFunction(dt_, sqrt_information_); + // return new Unicycle2DStateCostFunction(dt_, sqrt_information_); } } // namespace fuse_models diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index 646366ee3..d5c7dfc09 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -175,7 +175,7 @@ bool Unicycle3D::applyCallback(fuse_core::Transaction & transaction) // manager, in turn, makes calls to the generateMotionModel() function. try { // Now actually generate the motion model segments - timestamp_manager_.query(transaction, true); + timestamp_manager_.query(transaction, true); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM_THROTTLE( logger_, *clock_, 10.0 * 1000, @@ -320,8 +320,6 @@ void Unicycle3D::generateMotionModel( } else { state1 = base_state; } - std::cout << "state1:" << std::endl; - state1.print(std::cout); // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); @@ -354,8 +352,6 @@ void Unicycle3D::generateMotionModel( state2.vel_angular, state2.acc_linear); - std::cout << "state2:" << std::endl; - state2.print(std::cout); // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); @@ -384,16 +380,6 @@ void Unicycle3D::generateMotionModel( position1->data()[fuse_variables::Position3DStamped::Y] = state1.position.y(); position1->data()[fuse_variables::Position3DStamped::Z] = state1.position.z(); - // fuse_core::Quaternion q; - // q = Eigen::AngleAxisd(state1.orientation.x(), Eigen::Vector3d::UnitX()) * - // Eigen::AngleAxisd(state1.orientation.y(), Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(state1.orientation.z(), Eigen::Vector3d::UnitZ()); - - // orientation1->data()[fuse_variables::Orientation3DStamped::X] = q.x(); - // orientation1->data()[fuse_variables::Orientation3DStamped::Y] = q.y(); - // orientation1->data()[fuse_variables::Orientation3DStamped::Z] = q.z(); - // orientation1->data()[fuse_variables::Orientation3DStamped::W] = q.w(); - orientation1->data()[fuse_variables::Orientation3DStamped::X] = state1.orientation.x(); orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); @@ -418,15 +404,6 @@ void Unicycle3D::generateMotionModel( position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); - // q = Eigen::AngleAxisd(state2.orientation.x(), Eigen::Vector3d::UnitX()) * - // Eigen::AngleAxisd(state2.orientation.y(), Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(state2.orientation.z(), Eigen::Vector3d::UnitZ()); -// - // orientation2->data()[fuse_variables::Orientation3DStamped::X] = q.x(); - // orientation2->data()[fuse_variables::Orientation3DStamped::Y] = q.y(); - // orientation2->data()[fuse_variables::Orientation3DStamped::Z] = q.z(); - // orientation2->data()[fuse_variables::Orientation3DStamped::Z] = q.w(); - orientation2->data()[fuse_variables::Orientation3DStamped::X] = state2.orientation.x(); orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); @@ -621,7 +598,7 @@ void Unicycle3D::updateStateHistoryEstimates( void Unicycle3D::validateMotionModel( const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix16d & process_noise_covariance) + const fuse_core::Matrix15d & process_noise_covariance) { try { state1.validate(); diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp index 44e439c57..fd6db4cd9 100644 --- a/fuse_models/src/unicycle_3d_ignition.cpp +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -39,33 +39,34 @@ #include #include +#include #include #include #include #include #include #include -#include +#include #include #include -#include -#include -#include +#include +#include +#include #include -#include -#include +#include +#include #include #include #include #include // Register this motion model with ROS as a plugin. -PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle2DIgnition, fuse_core::SensorModel); +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DIgnition, fuse_core::SensorModel); namespace fuse_models { -Unicycle2DIgnition::Unicycle2DIgnition() +Unicycle3DIgnition::Unicycle3DIgnition() : fuse_core::AsyncSensorModel(1), started_(false), initial_transaction_sent_(false), @@ -74,7 +75,7 @@ Unicycle2DIgnition::Unicycle2DIgnition() { } -void Unicycle2DIgnition::initialize( +void Unicycle3DIgnition::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string & name, fuse_core::TransactionCallback transaction_callback) @@ -83,7 +84,7 @@ void Unicycle2DIgnition::initialize( fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); } -void Unicycle2DIgnition::onInit() +void Unicycle3DIgnition::onInit() { logger_ = interfaces_.get_node_logging_interface()->get_logger(); clock_ = interfaces_.get_node_clock_interface()->get_clock(); @@ -112,7 +113,7 @@ void Unicycle2DIgnition::onInit() interfaces_, params_.topic, params_.queue_size, - std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), + std::bind(&Unicycle3DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options ); @@ -123,7 +124,7 @@ void Unicycle2DIgnition::onInit() interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), std::bind( - &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, + &Unicycle3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), cb_group_ @@ -135,14 +136,14 @@ void Unicycle2DIgnition::onInit() interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), std::bind( - &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + &Unicycle3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), cb_group_ ); } -void Unicycle2DIgnition::start() +void Unicycle3DIgnition::start() { started_ = true; @@ -151,25 +152,30 @@ void Unicycle2DIgnition::start() // transaction immediately, if requested if (params_.publish_on_startup && !initial_transaction_sent_) { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); + tf2::Quaternion q; + q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; - pose.pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); - pose.pose.covariance[0] = params_.initial_sigma[0] * params_.initial_sigma[0]; - pose.pose.covariance[7] = params_.initial_sigma[1] * params_.initial_sigma[1]; - pose.pose.covariance[35] = params_.initial_sigma[2] * params_.initial_sigma[2]; + pose.pose.pose.position.z = params_.initial_state[2]; + pose.pose.pose.orientation.x = q.x(); + pose.pose.pose.orientation.y = q.y(); + pose.pose.pose.orientation.z = q.z(); + pose.pose.pose.orientation.w = q.w(); + for (size_t i = 0; i < 6; i++) { + pose.pose.covariance[i * 7] = params_.initial_sigma[i] * params_.initial_sigma[i]; + } sendPrior(pose); initial_transaction_sent_ = true; } } -void Unicycle2DIgnition::stop() +void Unicycle3DIgnition::stop() { started_ = false; } -void Unicycle2DIgnition::subscriberCallback( +void Unicycle3DIgnition::subscriberCallback( const geometry_msgs::msg::PoseWithCovarianceStamped & msg) { try { @@ -179,7 +185,7 @@ void Unicycle2DIgnition::subscriberCallback( } } -bool Unicycle2DIgnition::setPoseServiceCallback( +bool Unicycle3DIgnition::setPoseServiceCallback( rclcpp::Service::SharedPtr service, std::shared_ptr request_id, const fuse_msgs::srv::SetPose::Request::SharedPtr req) @@ -202,7 +208,7 @@ bool Unicycle2DIgnition::setPoseServiceCallback( return true; } -bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( +bool Unicycle3DIgnition::setPoseDeprecatedServiceCallback( rclcpp::Service::SharedPtr service, std::shared_ptr request_id, const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) @@ -222,7 +228,7 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Unicycle2DIgnition::process( +void Unicycle3DIgnition::process( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) { // Verify we are in the correct state to process set pose requests @@ -230,11 +236,12 @@ void Unicycle2DIgnition::process( throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || !std::isfinite(pose.pose.pose.position.z)) { throw std::invalid_argument( "Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + - std::to_string(pose.pose.pose.position.y) + ")."); + std::to_string(pose.pose.pose.position.y) + ", " + + std::to_string(pose.pose.pose.position.z) + ")."); } auto orientation_norm = std::sqrt( pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + @@ -249,12 +256,18 @@ void Unicycle2DIgnition::process( std::to_string(pose.pose.pose.orientation.z) + ", " + std::to_string(pose.pose.pose.orientation.w) + ")."); } - auto position_cov = fuse_core::Matrix2d(); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; + auto position_cov = fuse_core::Matrix3d(); + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; + // } + // } + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; if (!fuse_core::isSymmetric(position_cov)) { throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric position covariance matri\n " + + "Attempting to set the pose with a non-symmetric position covariance matrix\n " + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } if (!fuse_core::isPositiveDefinite(position_cov)) { @@ -262,14 +275,25 @@ void Unicycle2DIgnition::process( "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } - auto orientation_cov = fuse_core::Matrix1d(); - orientation_cov << pose.pose.covariance[35]; - if (orientation_cov(0) <= 0.0) { + auto orientation_cov = fuse_core::Matrix3d(); + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[3+i * 6 + j]; + // } + // } + orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + if (!fuse_core::isSymmetric(orientation_cov)) { throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite orientation covariance " - "matrix " + fuse_core::to_string(orientation_cov) + "."); + "Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + } + if (!fuse_core::isPositiveDefinite(orientation_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); } - // Tell the optimizer to reset before providing the initial state if (!params_.reset_service.empty()) { // Wait for the reset service @@ -304,79 +328,104 @@ void Unicycle2DIgnition::process( } } -void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Unicycle3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { const auto & stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped // message. The remaining dimensions are provided as parameters to the parameter server. - auto position = fuse_variables::Position2DStamped::make_shared(stamp, device_id_); + auto position = fuse_variables::Position3DStamped::make_shared(stamp, device_id_); position->x() = pose.pose.pose.position.x; position->y() = pose.pose.pose.position.y; - auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); - orientation->yaw() = fuse_core::getYaw( - pose.pose.pose.orientation.w, - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z); - auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); - linear_velocity->x() = params_.initial_state[3]; - linear_velocity->y() = params_.initial_state[4]; - auto angular_velocity = fuse_variables::VelocityAngular2DStamped::make_shared(stamp, device_id_); - angular_velocity->yaw() = params_.initial_state[5]; - auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared( - stamp, - device_id_); - linear_acceleration->x() = params_.initial_state[6]; - linear_acceleration->y() = params_.initial_state[7]; + position->z() = pose.pose.pose.position.z; + auto orientation = fuse_variables::Orientation3DStamped::make_shared(stamp, device_id_); + orientation->w() = pose.pose.pose.orientation.w; + orientation->x() = pose.pose.pose.orientation.x; + orientation->y() = pose.pose.pose.orientation.y; + orientation->z() = pose.pose.pose.orientation.z; + auto linear_velocity = fuse_variables::VelocityLinear3DStamped::make_shared(stamp, device_id_); + linear_velocity->x() = params_.initial_state[6]; + linear_velocity->y() = params_.initial_state[7]; + linear_velocity->z() = params_.initial_state[8]; + auto angular_velocity = fuse_variables::VelocityAngular3DStamped::make_shared(stamp, device_id_); + angular_velocity->roll() = params_.initial_state[9]; + angular_velocity->pitch() = params_.initial_state[10]; + angular_velocity->yaw() = params_.initial_state[11]; + auto linear_acceleration = fuse_variables::AccelerationLinear3DStamped::make_shared(stamp, device_id_); + linear_acceleration->x() = params_.initial_state[12]; + linear_acceleration->y() = params_.initial_state[13]; + linear_acceleration->z() = params_.initial_state[14]; // Create the covariances for each variable // The pose covariances are extracted from the provided PoseWithCovarianceStamped message. // The remaining covariances are provided as parameters to the parameter server. - auto position_cov = fuse_core::Matrix2d(); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; - auto orientation_cov = fuse_core::Matrix1d(); - orientation_cov << pose.pose.covariance[35]; - auto linear_velocity_cov = fuse_core::Matrix2d(); - linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, - 0.0, params_.initial_sigma[4] * params_.initial_sigma[4]; - auto angular_velocity_cov = fuse_core::Matrix1d(); - angular_velocity_cov << params_.initial_sigma[5] * params_.initial_sigma[5]; - auto linear_acceleration_cov = fuse_core::Matrix2d(); - linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7]; + auto position_cov = fuse_core::Matrix3d(); + auto orientation_cov = fuse_core::Matrix3d(); + auto linear_velocity_cov = fuse_core::Matrix3d(); + auto angular_velocity_cov = fuse_core::Matrix3d(); + auto linear_acceleration_cov = fuse_core::Matrix3d(); + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; + // orientation_cov(i, j) = pose.pose.covariance[(i + 3) * 6 + j + 3]; + // if (i == j) { + // linear_velocity_cov(i, j) = params_.initial_sigma[6 + i] * params_.initial_sigma[6 + i]; + // angular_velocity_cov(i, j) = params_.initial_sigma[9 + i] * params_.initial_sigma[9 + i]; + // linear_acceleration_cov(i, j) = params_.initial_sigma[12 + i] * params_.initial_sigma[12 + i]; + // } + // } + // } + + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + + orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + + linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, + 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, + 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; + + angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, + 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, + 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; + + linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, + 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, + 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; // Create absolute constraints for each variable - auto position_constraint = fuse_constraints::AbsolutePosition2DStampedConstraint::make_shared( + auto position_constraint = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( name(), *position, - fuse_core::Vector2d(position->x(), position->y()), + fuse_core::Vector3d(position->x(), position->y(), position->z()), position_cov); auto orientation_constraint = - fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( name(), *orientation, - fuse_core::Vector1d(orientation->yaw()), + fuse_core::Quaternion(orientation->w(), orientation->x(), orientation->y(), orientation->z()), orientation_cov); auto linear_velocity_constraint = - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( name(), *linear_velocity, - fuse_core::Vector2d(linear_velocity->x(), linear_velocity->y()), + fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), linear_velocity_cov); auto angular_velocity_constraint = - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( name(), *angular_velocity, - fuse_core::Vector1d(angular_velocity->yaw()), + fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), angular_velocity_cov); auto linear_acceleration_constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( name(), *linear_acceleration, - fuse_core::Vector2d(linear_acceleration->x(), linear_acceleration->y()), + fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), linear_acceleration_cov); // Create the transaction @@ -401,8 +450,9 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS logger_, "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() << ", x: " << position->x() << ", y: " - << position->y() << ", yaw: " << orientation->yaw() << - ")"); + << position->y() << ", z: " << position->z() + << ", roll: " << orientation->roll() + << ", pitch: " << orientation->pitch() + << ", yaw: " << orientation->yaw() << ")"); } - } // namespace fuse_models diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp index 88a4dbf33..8727eba47 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -62,7 +62,7 @@ Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( const fuse_variables::VelocityLinear3DStamped & velocity_linear2, const fuse_variables::VelocityAngular3DStamped & velocity_angular2, const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix16d & covariance) + const fuse_core::Matrix15d & covariance) : fuse_core::Constraint( // TODO: check if there is a constructor for these arguments source, {position1.uuid(), @@ -104,7 +104,8 @@ ceres::CostFunction * Unicycle3DStateKinematicConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // - return new ceres::AutoDiffCostFunction( new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); + return new ceres::AutoDiffCostFunction( + new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); // // which requires: // diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index f8e4347f5..e4825b941 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,6 +5,7 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition + # test_unicycle_3d_predict ) foreach(test_name ${TEST_TARGETS}) diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp new file mode 100644 index 000000000..59da3e516 --- /dev/null +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include + +#include +#include +#include + +#include +#include +// #include + +TEST(Predict, predictDirectVals) +{ + fuse_core::Vector3d position1 (0.0, 0.0, 0.0); + fuse_core::Quaternion orientation1 (1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + const double dt = 0.1; + fuse_core::Vector3d position2; + fuse_core::Quaternion orientation2; + fuse_core::Vector3d vel_linear2; + fuse_core::Vector3d vel_angular2; + fuse_core::Vector3d acc_linear2; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + fuse_core::Quaternion q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Use non-zero Y values + vel_linear1.y() = -1.0; + vel_angular1.z() = -1.570796327; + acc_linear1.y() = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(-0.105, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + + // TODO: continue with out of plane motion +} + +TEST(Predict, predictJacobians) +{ + // atm this test is doig nothing, we don't have a way to compute the jacobian analytically + fuse_core::Vector3d position1 (0.0, 0.0, 0.0); + fuse_core::Quaternion orientation1 (1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + + const double dt = 0.1; + fuse_core::Vector3d position2; + fuse_core::Quaternion orientation2; + fuse_core::Vector3d vel_linear2; + fuse_core::Vector3d vel_angular2; + fuse_core::Vector3d acc_linear2; + + // Predict and compute Jacobian using autodiff + using Jet = ceres::Jet; + const Jet jet_dt (dt); + const Jet jet_position1[3] {{position1.x(), 0}, {position1.y(), 1}, {position1.z(), 2}}; + const Jet jet_orientation1[3] {{orientation1.x(), 0}, {orientation1.y(), 1}, {orientation1.z(), 2}}; + const Jet jet_vel_linear1[3] {{vel_linear1.x(), 0}, {vel_linear1.y(), 1}, {vel_linear1.z(), 2}}; + const Jet jet_vel_angular1[3] {{vel_angular1.x(), 0}, {vel_angular1.y(), 1}, {vel_angular1.z(), 2}}; + const Jet jet_acc_linear1[3] {{acc_linear1.x(), 0}, {acc_linear1.y(), 1}, {acc_linear1.z(), 2}}; + + Jet jet_position2[3], jet_orientation2[3], jet_vel_linear2[3], jet_vel_angular2[3], jet_acc_linear2[3]; + + fuse_models::predict( + jet_position1, + jet_orientation1, + jet_vel_linear1, + jet_vel_angular1, + jet_acc_linear1, + jet_dt, + jet_position2, + jet_orientation2, + jet_vel_linear2, + jet_vel_angular2, + jet_acc_linear2); + + fuse_core::Matrix15d J_autodiff; + J_autodiff << jet_position2[0].v, jet_position2[1].v, jet_position2[2].v, + jet_orientation2[0].v, jet_orientation2[1].v, jet_orientation2[2].v, + jet_vel_linear2[0].v, jet_vel_linear2[1].v, jet_vel_linear2[2].v, + jet_vel_angular2[0].v, jet_vel_angular2[1].v, jet_vel_angular2[2].v, + jet_acc_linear2[0].v, jet_acc_linear2[1].v, jet_acc_linear2[2].v; + J_autodiff.transposeInPlace(); + + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + std::cout << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt); + // EXPECT_MATRIX_NEAR(J_autodiff, J_analytic, std::numeric_limits::epsilon()) + // << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt); + // << "\nAnalytic Jacobian =\n" << J_analytic.format(HeavyFmt); +} diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp index 6f1512dc5..60a98244b 100644 --- a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -42,12 +42,13 @@ #include #include #include -// #include TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); const double dt{0.1}; @@ -74,51 +75,52 @@ TEST(CostFunction, evaluateCostFunction) position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; - fuse_core::Vector8d residuals; + // fuse_core::Vector8d residuals; - const auto & block_sizes = cost_function.parameter_block_sizes(); - const auto num_parameter_blocks = block_sizes.size(); + // const auto & block_sizes = cost_function.parameter_block_sizes(); + // const auto num_parameter_blocks = block_sizes.size(); - const auto num_residuals = cost_function.num_residuals(); + // const auto num_residuals = cost_function.num_residuals(); - std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); + // std::vector J(num_parameter_blocks); + // std::vector jacobians(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { - J[i].resize(num_residuals, block_sizes[i]); - jacobians[i] = J[i].data(); - } + // for (size_t i = 0; i < num_parameter_blocks; ++i) { + // J[i].resize(num_residuals, block_sizes[i]); + // jacobians[i] = J[i].data(); + // } - EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); + // EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); - // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 - // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 - EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); + // // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 + // // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 + // EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); - // Check jacobians are correct using a gradient checker - ceres::NumericDiffOptions numeric_diff_options; +// // Check jacobians are correct using a gradient checker +// ceres::NumericDiffOptions numeric_diff_options; -#if CERES_VERSION_AT_LEAST(2, 1, 0) - std::vector parameterizations; - ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); -#else - ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); -#endif +// #if CERES_VERSION_AT_LEAST(2, 1, 0) +// std::vector parameterizations; +// ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); +// #else +// ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +// #endif - // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error - // is 5.26356e-10 - ceres::GradientChecker::ProbeResults probe_results; - // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, - // but Probe actually returns true and the jacobians are correct according to the - // gradient checker numeric differentiation - // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << - // probe_results.error_log; +// // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error +// // is 5.26356e-10 +// ceres::GradientChecker::ProbeResults probe_results; +// // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, +// // but Probe actually returns true and the jacobians are correct according to the +// // gradient checker numeric differentiation +// // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << +// // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); - + auto num_parameter_blocks = cost_function_autodiff.parameter_block_sizes().size(); + auto num_residuals = cost_function_autodiff.num_residuals(); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); std::vector jacobians_autodiff(num_parameter_blocks); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 0543cde55..3e7a91f30 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -404,7 +404,6 @@ void FixedLagSmoother::processQueue( return; } } - // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); diff --git a/fuse_tutorials/launch/range_sensor_tutorial.launch.py b/fuse_tutorials/launch/range_sensor_tutorial.launch.py index ce708483e..6a94db83e 100755 --- a/fuse_tutorials/launch/range_sensor_tutorial.launch.py +++ b/fuse_tutorials/launch/range_sensor_tutorial.launch.py @@ -41,7 +41,9 @@ def generate_launch_description(): output='screen', parameters=[PathJoinSubstitution([ pkg_dir, 'config', 'range_sensor_tutorial.yaml' - ])] + ])], + # prefix=["gdbserver localhost:3000"], + emulate_tty=True, ), Node( package='rviz2', diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 65f5810cc..6cc923be5 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -82,21 +82,28 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati int GlobalSize() const override { + // the manifold is the quaternion space, which is 4D return 4; } int LocalSize() const override { + // the local tangent space is the angle-axis space, which is 3D return 3; } + // the box-plus operator: given a quaternion x and a tangent vector delta, this function computes + // the perturbed quaternion x_plus_delta bool Plus( const double * x, const double * delta, double * x_plus_delta) const override { double q_delta[4]; + // exponential map: transform a delta in tangent space into a delta in the manifold ceres::AngleAxisToQuaternion(delta, q_delta); + // manifold group product operation (in this case is the quaternion product) + // this combines the delta with the current value ceres::QuaternionProduct(x, q_delta, x_plus_delta); return true; } From 9927ad02adeffcade4d026d061b4c89eb5f79477 Mon Sep 17 00:00:00 2001 From: giacomo Date: Fri, 3 Nov 2023 16:21:42 +0100 Subject: [PATCH 009/116] add normal_prior_pose_3d --- fuse_constraints/CMakeLists.txt | 4 + .../fuse_constraints/normal_prior_pose_3d.hpp | 106 ++++++++++++++ fuse_constraints/package.xml | 2 + ...olute_pose_3d_stamped_euler_constraint.cpp | 9 +- fuse_constraints/src/normal_prior_pose_3d.cpp | 138 ++++++++++++++++++ .../fuse_variables/orientation_3d_stamped.hpp | 2 +- 6 files changed, 257 insertions(+), 4 deletions(-) create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp create mode 100644 fuse_constraints/src/normal_prior_pose_3d.cpp diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 4ab4ede2f..bfdc35336 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -12,6 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(covariance_geometry_ros REQUIRED) find_package(fuse_core REQUIRED) find_package(fuse_graphs REQUIRED) find_package(fuse_variables REQUIRED) @@ -46,6 +47,7 @@ add_library(${PROJECT_NAME} src/normal_delta_pose_2d.cpp src/normal_prior_orientation_2d.cpp src/normal_prior_pose_2d.cpp + src/normal_prior_pose_3d.cpp src/relative_constraint.cpp src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp @@ -60,6 +62,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PUBLIC Boost::serialization + covariance_geometry_ros::covariance_geometry_ros Ceres::ceres Eigen3::Eigen fuse_core::fuse_core @@ -112,6 +115,7 @@ 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 + covariance_geometry_ros fuse_core fuse_graphs fuse_variables diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp new file mode 100644 index 000000000..364df3e90 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, 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_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2eul(b(3-6)^-1 * q)] || + * + * Here, the matrix A can be of variable size, thereby permitting the computation of errors for + * partial measurements. The vector b is a fixed-size 7x1. In case the user is interested in + * implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3D : public ceres::SizedCostFunction +{ +public: + /** + * @brief Construct a cost function instance + * + * The residual weighting matrix can vary in size, as this cost functor can be used to compute + * costs for partial vectors. The number of rows of A will be the number of dimensions for which + * you want to compute the error, and the number of columns in A will be fixed at 6. For example, + * if we just want to use the values of x, y and yaw, then \p A will be of size 3x6. + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + */ + NormalPriorPose3D(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector7d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index ed2bc9dc7..52a07a604 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -20,6 +20,7 @@ libceres-dev suitesparse + covariance_geometry_ros fuse_core fuse_graphs fuse_variables @@ -27,6 +28,7 @@ pluginlib rclcpp + covariance_geometry_ros fuse_core fuse_graphs fuse_variables diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index 9a3eddff5..c3e82076e 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -39,7 +39,8 @@ #include #include -#include +// #include +#include #include namespace fuse_constraints @@ -130,8 +131,10 @@ void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_, axes_)); + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_, axes_)); + + return new NormalPriorPose3D(sqrt_information_, mean_); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp new file mode 100644 index 000000000..a2fa6a900 --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath 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. + */ +#include +#include +#include + +#include +#include + +#include + +namespace fuse_constraints +{ + +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); + set_num_residuals(A_.rows()); +} + +bool NormalPriorPose3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + fuse_core::Vector6d full_residuals_vector; + full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x + full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y + full_residuals_vector[2] = parameters[0][2] - b_[2]; // position z + + // double * qb_inverse[4], q_or[4], q_res[4]; + + // qb_inverse = { + // b_(3), + // -b_(4), + // -b_(5), + // -b_(6) + // }; + + // q_or = { + // parameters[1][0], + // parameters[1][1], + // parameters[1][2], + // parameters[1][3] + // }; + + // ceres::QuaternionProduct(qb_inverse, q_or, q_res); + + Eigen::Map q_orientation_map(parameters[1]); + auto qb_inv = Eigen::Quaterniond(b_(3), b_(4), b_(5), b_(6)).conjugate(); + auto q_res = qb_inv * q_orientation_map; + + full_residuals_vector[3] = fuse_core::getRoll(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation roll + full_residuals_vector[4] = fuse_core::getPitch(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation pitch + full_residuals_vector[5] = fuse_core::getYaw(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation yaw + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_vector(residuals, num_residuals()); + residuals_vector = A_ * full_residuals_vector; + + if (jacobians != nullptr) { + // Jacobian of the position residuals wrt position parameters block (max 3x3) + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + + // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) + if (jacobians[1] != nullptr) { + // We make use of the chain rule of derivatives: + // First: compute the jacobian of the quaternion product wrt orientation parameters + fuse_core::Matrix4d dqprod_dq1; + dqprod_dq1(0, 0) = qb_inv.w(); + dqprod_dq1(0, 1) = -qb_inv.x(); + dqprod_dq1(0, 2) = -qb_inv.y(); + dqprod_dq1(0, 3) = -qb_inv.z(); + + dqprod_dq1(1, 0) = qb_inv.x(); + dqprod_dq1(1, 1) = qb_inv.w(); + dqprod_dq1(1, 2) = -qb_inv.z(); + dqprod_dq1(1, 3) = qb_inv.y(); + + dqprod_dq1(2, 0) = qb_inv.y(); + dqprod_dq1(2, 1) = qb_inv.z(); + dqprod_dq1(2, 2) = qb_inv.w(); + dqprod_dq1(2, 3) = -qb_inv.x(); + + dqprod_dq1(3, 0) = qb_inv.z(); + dqprod_dq1(3, 1) = -qb_inv.y(); + dqprod_dq1(3, 2) = qb_inv.x(); + dqprod_dq1(3, 3) = qb_inv.w(); + + // Second: compute the jacobian of the quat2eul function wrt the quaternion residual + Eigen::Matrix3_4d dquat2eul_dq; + covariance_geometry::jacobianQuaternionToRPY(q_res, dquat2eul_dq); + + // Third: apply the chain rule + Eigen::Map(jacobians[1], num_residuals(), 3) = A_.rightCols<3>() * dquat2eul_dq * dqprod_dq1; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 6cc923be5..54581e090 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -58,7 +58,7 @@ namespace fuse_variables /** * @brief A LocalParameterization class for 3D Orientations. * - * 3D orientations add and subtract nonlinearly. Additionally, the typcial 3D orientation + * 3D orientations add and subtract nonlinearly. Additionally, the typical 3D orientation * representation is a quaternion, which has 4 degrees of freedom to parameterize a 3D space. This * local parameterization uses the Rodrigues/angle-axis formulas to combine 3D rotations, along with * the appropriate "analytic" derivatives. From 5a3df7b81bd3e3923da629fa89e0e2ccee886e61 Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 6 Nov 2023 18:13:49 +0100 Subject: [PATCH 010/116] odometry 3d with partial measurements development --- ...olute_pose_3d_stamped_euler_constraint.hpp | 2 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 23 ++++--- fuse_constraints/src/normal_prior_pose_3d.cpp | 65 ++++++++++++------- .../fuse_models/common/sensor_proc.hpp | 24 ++++--- fuse_models/src/unicycle_3d_ignition.cpp | 3 +- 5 files changed, 75 insertions(+), 42 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index 1227f5829..94128acf1 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -100,7 +100,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint const fuse_variables::Position3DStamped & position, const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance, + const fuse_core::MatrixXd & covariance, const std::vector & linear_indices, const std::vector & angular_indices); diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index c3e82076e..782cce809 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -51,7 +51,7 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( const fuse_variables::Position3DStamped & position, const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance, + const fuse_core::MatrixXd & covariance, const std::vector & linear_indices, const std::vector & angular_indices) : fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT @@ -69,8 +69,8 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( std::copy(angular_indices.begin(), angular_indices.end(), std::back_inserter(total_indices)); // Compute the sqrt information of the provided cov matrix - fuse_core::Matrix6d sqrt_information = covariance.inverse().llt().matrixU(); - // std::cout << "sqrt_information = " << "\n" << sqrt_information << std::endl; + fuse_core::MatrixXd partial_sqrt_information = covariance.inverse().llt().matrixU(); + std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; // Assemble a mean vector and sqrt information matrix from the provided values, but in proper // Variable order @@ -82,21 +82,28 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( // and the columns are in the order defined by the variable. // build partial mean and information matrix + // TODO: find a way to insert the zeros inside the full mean vector mean_ = fuse_core::Vector7d::Zero(); sqrt_information_ = fuse_core::Matrix6d::Zero(); for (size_t i = 0; i < linear_indices.size(); ++i) { mean_(linear_indices[i]) = mean(linear_indices[i]); + sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); } mean_.tail(4) = mean.tail(4); - for (auto& i : total_indices) { - sqrt_information_.col(i) = sqrt_information.col(i); + // for (auto& i : total_indices) { + // sqrt_information_.col(i) = sqrt_information.col(i); + // } + for (size_t i = linear_indices.size(); i < total_indices.size(); ++i) { + size_t final_index = position.size() + angular_indices[i - linear_indices.size()]; + // mean_(final_index) = partial_mean(i); + sqrt_information_.col(final_index) = partial_sqrt_information.col(i); } - // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - // std::cout << "mean_ = " << mean_.transpose() << std::endl; + std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; + std::cout << "mean_ = " << mean_.transpose() << std::endl; } fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const @@ -121,7 +128,7 @@ void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const << " position variable: " << variables().at(0) << "\n" << " orientation variable: " << variables().at(1) << "\n" << " mean: " << mean().transpose() << "\n" - << " sqrt_info: " << sqrtInformation() << "\n"; + << " sqrt_info: \n" << sqrtInformation() << "\n"; if (loss()) { stream << " loss: "; diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index a2fa6a900..bc0653370 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -62,45 +62,39 @@ bool NormalPriorPose3D::Evaluate( full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y full_residuals_vector[2] = parameters[0][2] - b_[2]; // position z - // double * qb_inverse[4], q_or[4], q_res[4]; - - // qb_inverse = { - // b_(3), - // -b_(4), - // -b_(5), - // -b_(6) - // }; - - // q_or = { - // parameters[1][0], - // parameters[1][1], - // parameters[1][2], - // parameters[1][3] - // }; - - // ceres::QuaternionProduct(qb_inverse, q_or, q_res); - - Eigen::Map q_orientation_map(parameters[1]); + // Eigen::Map q_orientation_map(parameters[1]); + auto q_orientation_map = Eigen::Quaterniond(parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3]); auto qb_inv = Eigen::Quaterniond(b_(3), b_(4), b_(5), b_(6)).conjugate(); auto q_res = qb_inv * q_orientation_map; full_residuals_vector[3] = fuse_core::getRoll(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation roll full_residuals_vector[4] = fuse_core::getPitch(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation pitch full_residuals_vector[5] = fuse_core::getYaw(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation yaw + + std::cout << "q_res: " << q_res.w() << ", " << q_res.x() << ", " << q_res.y() << ", " << q_res.z() << std::endl; + std::cout << "full_residuals_vector: " << full_residuals_vector.transpose() << std::endl; // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map residuals_vector(residuals, num_residuals()); + Eigen::Map residuals_vector(residuals, num_residuals()); residuals_vector = A_ * full_residuals_vector; if (jacobians != nullptr) { // Jacobian of the position residuals wrt position parameters block (max 3x3) if (jacobians[0] != nullptr) { - Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + Eigen::Map j0_map(jacobians[0], num_residuals(), 3); + j0_map.setZero(); + j0_map.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + // j0_map.leftCols<3>() = A_.block<3, 3>(0, 0); + j0_map.applyOnTheLeft(A_); } // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) if (jacobians[1] != nullptr) { + + Eigen::Map j1_map(jacobians[1], num_residuals(), 4); + j1_map.setZero(); + // We make use of the chain rule of derivatives: // First: compute the jacobian of the quaternion product wrt orientation parameters fuse_core::Matrix4d dqprod_dq1; @@ -125,12 +119,37 @@ bool NormalPriorPose3D::Evaluate( dqprod_dq1(3, 3) = qb_inv.w(); // Second: compute the jacobian of the quat2eul function wrt the quaternion residual - Eigen::Matrix3_4d dquat2eul_dq; + Eigen::Matrix dquat2eul_dq; covariance_geometry::jacobianQuaternionToRPY(q_res, dquat2eul_dq); + Eigen::PermutationMatrix<4> perm; + perm.indices() = {1, 2, 3, 0}; + dquat2eul_dq.applyOnTheRight(perm); // Third: apply the chain rule - Eigen::Map(jacobians[1], num_residuals(), 3) = A_.rightCols<3>() * dquat2eul_dq * dqprod_dq1; + // j1_map.rightCols<3>() = (A_.block<3, 3>(3, 3) * dquat2eul_dq * dqprod_dq1).transpose(); + j1_map.block<3, 4>(3, 0) = dquat2eul_dq * dqprod_dq1; + j1_map.applyOnTheLeft(A_); + } + + std::cout << "residuals: " << residuals_vector.transpose() << std::endl; + std::cout << "jacobians[0] : " << std::endl; + for (size_t i = 0; i < 24; i++) + { + if (i % 3 == 0) + std::cout << std::endl; + std::cout << jacobians[0][i] << ", "; } + std::cout << std::endl; + std::cout << "eigen jacobians[0]: " << std::endl << Eigen::Map(jacobians[0], num_residuals(), 3) << std::endl; + + std::cout << "jacobians[1] : " << std::endl; + for (size_t i = 0; i < 24; i++) + { + if (i % 4 == 0) + std::cout << std::endl; + std::cout << jacobians[1][i] << ", "; + } + std::cout << "eigen jacobians[1]: " << std::endl << Eigen::Map(jacobians[1], num_residuals(), 4) << std::endl; } return true; } diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 462c337b7..b926fc27c 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -218,14 +218,14 @@ inline void populatePartialMeasurement( fuse_core::MatrixXd & covariance_partial) { covariance_partial.setZero(); - for (auto & i : indices) { - covariance_partial.col(i) = covariance_full.col(i); - } - // for (size_t r = 0; r < indices.size(); ++r) { - // for (size_t c = 0; c < indices.size(); ++c) { - // covariance_partial(r, c) = covariance_full(indices[r], indices[c]); - // } + // for (auto & i : indices) { + // covariance_partial.col(i) = covariance_full.col(i); // } + for (size_t r = 0; r < indices.size(); ++r) { + for (size_t c = 0; c < indices.size(); ++c) { + covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + } + } } /** @@ -422,6 +422,8 @@ inline bool processAbsolutePoseWithCovariance( return false; } } + std::cout << "Pose mean partial: \n" << pose_mean_partial << std::endl; + std::cout << "Pose covariance partial: \n" << pose_covariance_partial << std::endl; // Create an absolute pose constraint auto constraint = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( @@ -520,9 +522,13 @@ inline bool processAbsolutePose3DWithCovariance( Eigen::Map cov_map(pose.pose.covariance.data()); fuse_core::Matrix6d pose_covariance = cov_map; // TODO check how to avoid this to not doing copies + const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); + fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement(pose_covariance, indices, pose_covariance_partial); + if (validate) { try { - validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); + validatePartialMeasurement(pose_mean, pose_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -539,7 +545,7 @@ inline bool processAbsolutePose3DWithCovariance( *position, *orientation, pose_mean, - pose_covariance, + pose_covariance_partial, position_indices, orientation_indices); diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp index fd6db4cd9..9c0167cc5 100644 --- a/fuse_models/src/unicycle_3d_ignition.cpp +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -153,7 +153,8 @@ void Unicycle3DIgnition::start() if (params_.publish_on_startup && !initial_transaction_sent_) { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); tf2::Quaternion q; - q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); + // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); + q.setEuler(params_.initial_state[5], params_.initial_state[4], params_.initial_state[3]); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; From c065595cc91b0426a8cd9472672d9743544b2d9f Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 8 Nov 2023 17:13:43 +0000 Subject: [PATCH 011/116] clean up: removed euler constraints --- ...olute_pose_3d_stamped_euler_constraint.hpp | 184 ------------------ ...elta_orientation_3d_euler_cost_functor.hpp | 175 ----------------- ...ormal_delta_pose_3d_euler_cost_functor.hpp | 162 --------------- ...ormal_prior_pose_3d_euler_cost_functor.hpp | 142 -------------- ...ative_pose_3d_stamped_euler_constraint.hpp | 174 ----------------- ...olute_pose_3d_stamped_euler_constraint.cpp | 150 -------------- ...ative_pose_3d_stamped_euler_constraint.cpp | 137 ------------- 7 files changed, 1124 deletions(-) delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp delete mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp delete mode 100644 fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp delete mode 100644 fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp deleted file mode 100644 index 94128acf1..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ -#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - - -namespace fuse_constraints -{ - -/** - * @brief A constraint that represents either prior information about a 3D pose, or a direct - * measurement of the 3D pose. - * - * A 3D pose is the combination of a 3D position and a 3D orientation variable (in rpy). As a convenience, - * this class applies an absolute constraint on both variables at once. This type of constraint - * arises in many situations. In mapping it is common to define the very first pose as the origin. - * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. - * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map - * measurements). This constraint holds the measured 3D pose and the measurement - * uncertainty/covariance. Orientations are represented as quaternions. - */ -class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint -{ -public: - FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint) - using Euler = fuse_variables::Orientation3DStamped::Euler; - - /** - * @brief Default constructor - */ - AbsolutePose3DStampedEulerConstraint() = default; - - /** - * @brief Create a constraint using a measurement/prior of the 3D pose - * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] position The variable representing the position components of the pose - * @param[in] orientation The variable representing the orientation components of the pose - * @param[in] mean The measured/prior pose as a vector - * (7x1 vector: x, y, z, qw, qx, qy, qz) - * @param[in] covariance The measurement/prior covariance (max 6x6 matrix: x, y, z, qx, qy, qz) - * @param[in] linear_indices The set of indices corresponding to the measured position - * dimensions e.g. "{fuse_variables::Position3DStamped::X, - * fuse_variables::Position3DStamped::Y, fuse_variables::Position3DStamped::Z}" - * @param[in] angular_indices The set of indices corresponding to the measured orientation - * dimensions e.g. "{fuse_variables::Orientation2DStamped::ROLL, - * fuse_variables::Orientation2DStamped::PITCH, fuse_variables::Orientation2DStamped::YAW}" - */ - AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & linear_indices, - const std::vector & angular_indices); - - /** - * @brief Destructor - */ - virtual ~AbsolutePose3DStampedEulerConstraint() = default; - - /** - * @brief Read-only access to the measured/prior vector of mean values. - * - * Order is (x, y, z, qw, qx, qy, qz) - */ - const fuse_core::Vector7d & mean() const {return mean_;} - - /** - * @brief Read-only access to the square root information matrix. - * - * Order is (x, y, z, qx, qy, qz) - */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} - - /** - * @brief Compute the measurement covariance matrix. - * - * Order is (x, y, z, qx, qy, qz) - */ - fuse_core::Matrix6d covariance() const; - - /** - * @brief Print a human-readable description of the constraint 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 Construct an instance of this constraint's cost function - * - * The function caller will own the new cost function instance. It is the responsibility of the - * caller to delete the cost function object when it is no longer needed. If the pointer is - * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the - * pointer and delete it during destruction. - * - * @return A base pointer to an instance of a derived CostFunction. - */ - ceres::CostFunction * costFunction() const override; - - - -protected: - fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable - fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix - std::vector axes_; //!< The axes to use for the orientation error. - -private: - // Allow Boost Serialization access to private methods - friend class boost::serialization::access; - - /** - * @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 - void serialize(Archive & archive, const unsigned int /* version */) - { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; - archive & axes_; - } -}; - -} // namespace fuse_constraints - -BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint); - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp deleted file mode 100644 index f86ed9c7f..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.hpp +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, 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_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_HPP_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_HPP_ - -#include -#include - -#include -#include -#include -#include - - -namespace fuse_constraints -{ - -/** - * @brief Implements a cost function that models a difference between 3D orientation variables - * (quaternion, but converted in RPY for residual computation) - * - * The cost function is of the form: - * - * || ||^2 - * cost(x) = || A * quat2rpy(b^-1 * q1^-1 * q2) || - * || || - * - * where the matrix A and the vector b are fixed, and q1 and q2 are the variables, represented as - * quaternions. The quat2rpy function converts a quaternion into a euler angles vector. - * The A matrix is applied to the euler angles vector. - * - * In case the user is interested in implementing a cost function of the form - * - * cost(X) = (X - mu)^T S^{-1} (X - mu) - * - * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the - * square root information matrix (the inverse of the covariance). - */ -class NormalDeltaOrientation3DEulerCostFunctor -{ -public: - FUSE_MAKE_ALIGNED_OPERATOR_NEW() - using Euler = fuse_variables::Orientation3DStamped::Euler; - - /** - * @brief Construct a cost function instance - * - * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z) - * @param[in] b The measured change between the two orientation variables (quaternion) - * @param[in] axes The axes to be considered in the cost function - */ - NormalDeltaOrientation3DEulerCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b, - std::vector axes) - : A_(A), - b_(b), - axes_(axes) - { - } - - /** - * @brief Evaluate the cost function. Used by the Ceres optimization engine. - */ - template - bool operator()(const T * const orientation1, const T * const orientation2, T * residuals) const - { - using fuse_variables::Orientation3DStamped; - - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; - - T difference[4]; - ceres::QuaternionProduct(orientation1_inverse, orientation2, difference); - T error[4]; - ceres::QuaternionProduct(observation_inverse, difference, error); - // instead of QuaternionToAngleAxis we implement QuaternionToRPY - // and filter the result based on the requested measurement axes - // ceres::QuaternionToAngleAxis(error, residuals); - - for (size_t i = 0; i < axes_.size(); ++i) { - T angle; - switch (axes_[i]) { - case Euler::ROLL: - { - angle = fuse_core::getRoll( - error[0], error[1], error[2], error[3]); - break; - } - case Euler::PITCH: - { - angle = - fuse_core::getPitch( - error[0], error[1], error[2], error[3]); - break; - } - case Euler::YAW: - { - angle = - fuse_core::getYaw( - error[0], error[1], error[2], error[3]); - break; - } - default: - { - throw std::runtime_error( - "The provided axis specified is unknown. " - "I should probably be more informative here"); - } - } - residuals[i] = angle; - } - // Scale the residuals by the square root information matrix to account for the measurement - // uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); - residuals_map.applyOnTheLeft(A_.template cast()); - - return true; - } - -private: - fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix - fuse_core::Vector4d b_; //!< The measured difference between orientation1 and orientation2 - std::vector axes_; //!< The axes to use for the orientation error. -}; - -} // namespace fuse_constraints - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp deleted file mode 100644 index f2b72b19a..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ - -#include - -#include -#include -#include -#include -#include - - -namespace fuse_constraints -{ - -/** - * @brief Implements a cost function that models a difference between 3D pose variables with orientation in RPY. - * - * A single pose involves two variables: a 3D position and a 3D orientation. This cost function - * computes the difference using standard 3D transformation math: - * - * cost(x) = || A * [ q1^-1 * (p2 - p1) - b(0:2) ] ||^2 - * || [ quat2rpy(b(3:6)^-1 * q1^-1 * q2) ] || - * - * where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, - * and the matrix A and the vector b are fixed. In case the user is interested in implementing a - * cost function of the form: - * - * cost(X) = (X - mu)^T S^{-1} (X - mu) - * - * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the - * square root information matrix (the inverse of the covariance). - * - * Note that the cost function's quaternion components are only concerned with the imaginary - * components (qx, qy, qz). - */ -class NormalDeltaPose3DEulerCostFunctor -{ -public: - FUSE_MAKE_ALIGNED_OPERATOR_NEW() - using Euler = fuse_variables::Orientation3DStamped::Euler; - - /** - * @brief Constructor - * - * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (dx, dy, dz, dqx, dqy, dqz) - * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) - * @param[in] axes The axes to use for the orientation error. Defaults to all three axes. - */ - NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, - const fuse_core::Vector7d & b, - const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); - - /** - * @brief Compute the cost values/residuals using the provided variable/parameter values - */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; - -private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix - fuse_core::Vector7d b_; //!< The measured difference between variable pose1 and variable pose2 - NormalDeltaOrientation3DEulerCostFunctor orientation_functor_; -}; - -NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector7d & b, - const std::vector & axes) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>(), axes) // Orientation residuals will - // not be scaled -{ -} - -template -bool NormalDeltaPose3DEulerCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const -{ - // Compute the position delta between pose1 and pose2 - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - T position_delta[3] = - { - position2[0] - position1[0], - position2[1] - position1[1], - position2[2] - position1[2] - }; - T position_delta_rotated[3]; - ceres::QuaternionRotatePoint( - orientation1_inverse, - position_delta, - position_delta_rotated); - - // Compute the first three residual terms as (position_delta - b) - residual[0] = position_delta_rotated[0] - T(b_[0]); - residual[1] = position_delta_rotated[1] - T(b_[1]); - residual[2] = position_delta_rotated[2] - T(b_[2]); - - // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation1, orientation2, &residual[3]); - - // Map it to Eigen, and weight it - Eigen::Map> residual_map(residual); - residual_map.applyOnTheLeft(A_.template cast()); - - return true; -} - -} // namespace fuse_constraints - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp deleted file mode 100644 index 51bbbd186..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ - -#include - -#include -#include -#include -#include -#include - - -namespace fuse_constraints -{ - -/** - * @brief Create a prior cost function on both the 3D position and orientation variables at once. - * - * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost - * function that applies a prior constraint on both the 3D position and orientation variables at - * once. - * - * The cost function is of the form: - * - * cost(x) = || A * [ p - b(0:2) ] ||^2 - * || [ rpy - b(3:6) ] || - * - * where, the matrix A and the vector b are fixed, p is the position variable, and rpy is the - * orientation variable. In case the user is interested in implementing a cost function of the form - * - * cost(X) = (X - mu)^T S^{-1} (X - mu) - * - * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the - * square root information matrix (the inverse of the covariance). - */ -class NormalPriorPose3DEulerCostFunctor -{ -public: - FUSE_MAKE_ALIGNED_OPERATOR_NEW() - using Euler = fuse_variables::Orientation3DStamped::Euler; - - /** - * @brief Construct a cost function instance - * - * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, roll, pitch, yaw) - * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) - * @param[in] axes The axes to use for the orientation error. Defaults to all three axes. - */ - NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, - const fuse_core::Vector7d & b, - const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); - - /** - * @brief Evaluate the cost function. Used by the Ceres optimization engine. - */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; - -private: - fuse_core::MatrixXd A_; - fuse_core::Vector7d b_; - - NormalPriorOrientation3DEulerCostFunctor orientation_functor_; -}; - -NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector7d & b, - const std::vector & axes) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>(), axes) // Delta will not be scaled -{ -} - -template -bool NormalPriorPose3DEulerCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const -{ - // Compute the position error - residual[0] = position[0] - T(b_(0)); - residual[1] = position[1] - T(b_(1)); - residual[2] = position[2] - T(b_(2)); - - // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation, &residual[3]); - // for (size_t i = 0; i < 6; ++i) - // { - // std::cout << "residuals before scaling" << std::endl; - // std::cout << residual[i] << std::endl; - // } - // Scale the residuals by the square root information matrix to account for - // the measurement uncertainty. - Eigen::Map> residual_map(residual); - residual_map.applyOnTheLeft(A_.template cast()); - // for (size_t i = 0; i < 6; ++i) - // { - // std::cout << "residuals after scaling" << std::endl; - // std::cout << residual[i] << std::endl; - // } - // std::cout << std::endl; - return true; -} - -} // namespace fuse_constraints - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp deleted file mode 100644 index 6ead861a4..000000000 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ /dev/null @@ -1,174 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ -#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - - -namespace fuse_constraints -{ - -/** - * @brief A constraint that represents a measurement on the difference between two 3D poses - * with orientation in rpy. - * - * This type of constraint arises in many situations. Many types of incremental odometry - * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This - * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. - */ -class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint -{ -public: - FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedEulerConstraint) - using Euler = fuse_variables::Orientation3DStamped::Euler; - - /** - * @brief Default constructor - */ - RelativePose3DStampedEulerConstraint() = default; - - /** - * @brief Constructor - * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] position1 The variable representing the position components of the first pose - * @param[in] orientation1 The variable representing the orientation components of the first pose - * @param[in] position2 The variable representing the position components of the second pose - * @param[in] orientation2 The variable representing the orientation components of the second pose - * @param[in] delta The measured change in the pose (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) - * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) - * @param[in] linear_indices The indices of the linear variables in the variable vector - * @param[in] angular_indices The indices of the angular variables in the variable vector - */ - RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance, - const std::vector & linear_indices, - const std::vector & angular_indices); - - /** - * @brief Destructor - */ - virtual ~RelativePose3DStampedEulerConstraint() = default; - - /** - * @brief Read-only access to the measured pose change. - */ - const fuse_core::Vector7d & delta() const {return delta_;} - - /** - * @brief Read-only access to the square root information matrix. - */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} - - /** - * @brief Compute the measurement covariance matrix. - * - * Order is (x, y, z, roll, pitch, yaw) - */ - fuse_core::Matrix6d covariance() const; - - /** - * @brief Print a human-readable description of the constraint 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 Access the cost function for this constraint - * - * The function caller will own the new cost function instance. It is the responsibility of the - * caller to delete the cost function object when it is no longer needed. If the pointer is - * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the - * pointer and delete it during destruction. - * - * @return A base pointer to an instance of a derived CostFunction. - */ - ceres::CostFunction * costFunction() const override; - -protected: - fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) - fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the - //!< covariance matrix) - std::vector axes_; //!< The axes to use for the orientation error. - -private: - // Allow Boost Serialization access to private methods - friend class boost::serialization::access; - - /** - * @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 - void serialize(Archive & archive, const unsigned int /* version */) - { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; - archive & axes_; - } -}; - -} // namespace fuse_constraints - -BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedEulerConstraint); - -#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp deleted file mode 100644 index 782cce809..000000000 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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. - */ -#include -#include - -#include -#include - -#include -#include -// #include -#include -#include - -namespace fuse_constraints -{ - -AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT -{ - axes_.resize(angular_indices.size()); - std::transform( - angular_indices.begin(), angular_indices.end(), axes_.begin(), - [](const size_t index) - {return static_cast(index + 1UL);}); - - // merge indices - std::vector total_indices; - total_indices.reserve(linear_indices.size() + angular_indices.size()); - std::copy(linear_indices.begin(), linear_indices.end(), std::back_inserter(total_indices)); - std::copy(angular_indices.begin(), angular_indices.end(), std::back_inserter(total_indices)); - - // Compute the sqrt information of the provided cov matrix - fuse_core::MatrixXd partial_sqrt_information = covariance.inverse().llt().matrixU(); - std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; - - // Assemble a mean vector and sqrt information matrix from the provided values, but in proper - // Variable order - // - // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 - // If we are measuring a subset of dimensions, we only want to produce costs for the measured - // dimensions. But the variable vectors will be full sized. We can make this all work out by - // creating a non-square A, where each row computes a cost for one measured dimensions, - // and the columns are in the order defined by the variable. - - // build partial mean and information matrix - // TODO: find a way to insert the zeros inside the full mean vector - mean_ = fuse_core::Vector7d::Zero(); - sqrt_information_ = fuse_core::Matrix6d::Zero(); - - for (size_t i = 0; i < linear_indices.size(); ++i) { - mean_(linear_indices[i]) = mean(linear_indices[i]); - sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); - } - - mean_.tail(4) = mean.tail(4); - - // for (auto& i : total_indices) { - // sqrt_information_.col(i) = sqrt_information.col(i); - // } - for (size_t i = linear_indices.size(); i < total_indices.size(); ++i) { - size_t final_index = position.size() + angular_indices[i - linear_indices.size()]; - // mean_(final_index) = partial_mean(i); - sqrt_information_.col(final_index) = partial_sqrt_information.col(i); - } - - std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - std::cout << "mean_ = " << mean_.transpose() << std::endl; -} - -fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const -{ - // We want to compute: - // cov = (sqrt_info' * sqrt_info)^-1 - // With some linear algebra, we can swap the transpose and the inverse. - // cov = (sqrt_info^-1) * (sqrt_info^-1)' - // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. - // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). - // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. - auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); - fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); - return pinv * pinv.transpose(); -} - -void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const -{ - stream << type() << "\n" - << " source: " << source() << "\n" - << " uuid: " << uuid() << "\n" - << " position variable: " << variables().at(0) << "\n" - << " orientation variable: " << variables().at(1) << "\n" - << " mean: " << mean().transpose() << "\n" - << " sqrt_info: \n" << sqrtInformation() << "\n"; - - if (loss()) { - stream << " loss: "; - loss()->print(stream); - } -} - -ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const -{ - // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_, axes_)); - - return new NormalPriorPose3D(sqrt_information_, mean_); -} - -} // namespace fuse_constraints - -BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp deleted file mode 100644 index 10317e0c7..000000000 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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. - */ -#include - -#include - -#include -#include -#include -#include - -namespace fuse_constraints -{ - -RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}) // NOLINT -{ - axes_.resize(angular_indices.size()); - std::transform( - angular_indices.begin(), angular_indices.end(), axes_.begin(), - [](const size_t index) - {return static_cast(index + 3UL);}); - size_t total_variable_size = position1.size() + orientation1.size(); - size_t total_indices = linear_indices.size() + angular_indices.size(); - - assert(delta.rows() == static_cast(total_indices)); - assert(covariance.rows() == static_cast(total_indices)); - assert(covariance.cols() == static_cast(total_indices)); - - // Compute the sqrt information of the provided cov matrix - fuse_core::MatrixXd sqrt_information = covariance.inverse().llt().matrixU(); - - // Assemble a delta vector and sqrt information matrix from the provided values, but in proper - // Variable order - // - // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 - // If we are measuring a subset of dimensions, we only want to produce costs for the measured - // dimensions. But the variable vectors will be full sized. We can make this all work out by - // creating a non-square A, where each row computes a cost for one measured dimensions, - // and the columns are in the order defined by the variable. - delta_ = fuse_core::Vector7d::Zero(); - sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); - for (size_t i = 0; i < linear_indices.size(); ++i) { - // build the partial translation delta and sqrt information - delta_(linear_indices[i]) = delta(i); - sqrt_information_.col(linear_indices[i]) = sqrt_information.col(i); - } - - // copy the full quaternion into the delta vector - delta_.tail(4) = delta.tail(4); - - for (size_t i = linear_indices.size(); i < total_indices; ++i) { - size_t final_index = position1.size() + angular_indices[i - linear_indices.size()]; - // build the partial rotation sqrt information - sqrt_information_.col(final_index) = sqrt_information.col(i); - } -} - -fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const -{ - // We want to compute: - // cov = (sqrt_info' * sqrt_info)^-1 - // With some linear algebra, we can swap the transpose and the inverse. - // cov = (sqrt_info^-1) * (sqrt_info^-1)' - // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. - // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). - // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. - auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); - fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); - return pinv * pinv.transpose(); -} - -void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const -{ - stream << type() << "\n" - << " source: " << source() << "\n" - << " uuid: " << uuid() << "\n" - << " position1 variable: " << variables().at(0) << "\n" - << " orientation1 variable: " << variables().at(1) << "\n" - << " position2 variable: " << variables().at(2) << "\n" - << " orientation2 variable: " << variables().at(3) << "\n" - << " delta: " << delta().transpose() << "\n" - << " sqrt_info: " << sqrtInformation() << "\n"; -} - -ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const -{ - return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_, axes_)); -} - -} // namespace fuse_constraints - -BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedEulerConstraint, fuse_core::Constraint); From 38cae760b18d4a5222ccc25792e30eae50419c4f Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 8 Nov 2023 17:15:34 +0000 Subject: [PATCH 012/116] 3D pose absolute and relative constraints with partial measure support --- .../absolute_pose_3d_stamped_constraint.hpp | 28 +++++++-- .../normal_delta_pose_3d_cost_functor.hpp | 27 +++++---- .../normal_prior_pose_3d_cost_functor.hpp | 24 ++++---- .../relative_pose_3d_stamped_constraint.hpp | 32 ++++++++-- .../absolute_pose_3d_stamped_constraint.cpp | 59 ++++++++++++++++++- .../relative_pose_3d_stamped_constraint.cpp | 59 ++++++++++++++++++- 6 files changed, 192 insertions(+), 37 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 51238c8b7..2727fd6f8 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -94,6 +94,25 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector7d & mean, const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose (version for partial measurements) + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior pose as a vector + * (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] variable_indices The indices of the measured variables + */ + AbsolutePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector7d & mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); /** * @brief Destructor @@ -112,17 +131,14 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qx, qy, qz) */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. * * Order is (x, y, z, qx, qy, qz) */ - fuse_core::Matrix6d covariance() const - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } + fuse_core::Matrix6d covariance() const; /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -145,7 +161,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint protected: fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable - fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: // Allow Boost Serialization access to private methods diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index ace413e51..073cfb945 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -35,6 +35,7 @@ #define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_HPP_ #include +#include #include #include @@ -78,7 +79,7 @@ class NormalDeltaPose3DCostFunctor * order (dx, dy, dz, dqx, dqy, dqz) * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) */ - NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalDeltaPose3DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values @@ -92,7 +93,7 @@ class NormalDeltaPose3DCostFunctor T * residual) const; private: - fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root //!< information matrix fuse_core::Vector7d b_; //!< The measured difference between variable pose1 and variable pose2 @@ -100,13 +101,15 @@ class NormalDeltaPose3DCostFunctor }; NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor( - const fuse_core::Matrix6d & A, + const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b) : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will // not be scaled { + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); } template @@ -117,6 +120,8 @@ bool NormalDeltaPose3DCostFunctor::operator()( const T * const orientation2, T * residual) const { + T full_residuals[6]; + // Compute the position delta between pose1 and pose2 T orientation1_inverse[4] = { @@ -138,16 +143,18 @@ bool NormalDeltaPose3DCostFunctor::operator()( position_delta_rotated); // Compute the first three residual terms as (position_delta - b) - residual[0] = position_delta_rotated[0] - T(b_[0]); - residual[1] = position_delta_rotated[1] - T(b_[1]); - residual[2] = position_delta_rotated[2] - T(b_[2]); + full_residuals[0] = position_delta_rotated[0] - T(b_[0]); + full_residuals[1] = position_delta_rotated[1] - T(b_[1]); + full_residuals[2] = position_delta_rotated[2] - T(b_[2]); // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation1, orientation2, &residual[3]); + orientation_functor_(orientation1, orientation2, &full_residuals[3]); - // Map it to Eigen, and weight it - Eigen::Map> residual_map(residual); - residual_map.applyOnTheLeft(A_.template cast()); + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; return true; } diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index f8d20f96f..5f536eb17 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -35,6 +35,7 @@ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_HPP_ #include +#include #include #include @@ -79,7 +80,7 @@ class NormalPriorPose3DCostFunctor * order (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalPriorPose3DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -88,19 +89,21 @@ class NormalPriorPose3DCostFunctor bool operator()(const T * const position, const T * const orientation, T * residual) const; private: - fuse_core::Matrix6d A_; + fuse_core::MatrixXd A_; fuse_core::Vector7d b_; NormalPriorOrientation3DCostFunctor orientation_functor_; }; NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor( - const fuse_core::Matrix6d & A, + const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b) : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); } template @@ -108,19 +111,20 @@ bool NormalPriorPose3DCostFunctor::operator()( const T * const position, const T * const orientation, T * residual) const { + T full_residuals[6]; // Compute the position error - residual[0] = position[0] - T(b_(0)); - residual[1] = position[1] - T(b_(1)); - residual[2] = position[2] - T(b_(2)); + full_residuals[0] = position[0] - T(b_(0)); + full_residuals[1] = position[1] - T(b_(1)); + full_residuals[2] = position[2] - T(b_(2)); // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation, &residual[3]); + orientation_functor_(orientation, &full_residuals[3]); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residual_map(residual); - residual_map.applyOnTheLeft(A_.template cast()); - + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; return true; } diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index 21155fc4c..49ac524e6 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -93,6 +93,29 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation2, const fuse_core::Vector7d & delta, const fuse_core::Matrix6d & covariance); + + /** + * @brief Constructor (version for partial measurements) + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose + * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] variable_indices The indices of the measured variables + */ + RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); /** * @brief Destructor @@ -107,15 +130,12 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. */ - fuse_core::Matrix6d covariance() const - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } + fuse_core::MatrixXd covariance() const; /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -138,7 +158,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint protected: fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) - fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix (derived from the + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) private: diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index cc64e8d75..983119a49 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -56,6 +56,58 @@ AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( { } +AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector7d & mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(mean) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } + + // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; + // std::cout << "mean_ = " << mean_.transpose() << std::endl; +} + +fuse_core::Matrix6d AbsolutePose3DStampedConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + + void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const { stream << type() << "\n" @@ -64,7 +116,7 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const << " position variable: " << variables().at(0) << "\n" << " orientation variable: " << variables().at(1) << "\n" << " mean: " << mean().transpose() << "\n" - << " sqrt_info: " << sqrtInformation() << "\n"; + << " sqrt_info: \n" << sqrtInformation() << "\n"; if (loss()) { stream << " loss: "; @@ -74,8 +126,9 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); + return new ceres::AutoDiffCostFunction( + new NormalPriorPose3DCostFunctor(sqrt_information_, mean_), + sqrt_information_.rows()); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 7d627e117..c76a6353f 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -59,6 +59,60 @@ RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( { } +RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(delta) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } + // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; + // std::cout << "mean_ = " << mean_.transpose() << std::endl; +} + +fuse_core::MatrixXd RelativePose3DStampedConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + void RelativePose3DStampedConstraint::print(std::ostream & stream) const { stream << type() << "\n" @@ -74,8 +128,9 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_), + sqrt_information_.rows()); } } // namespace fuse_constraints From 5f037ce1905a8b8045da285ba4295acd2945f9d0 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 8 Nov 2023 17:17:11 +0000 Subject: [PATCH 013/116] clean up: removed euler constraints plugin from cmake --- fuse_constraints/CMakeLists.txt | 2 -- fuse_constraints/fuse_plugins.xml | 10 ---------- 2 files changed, 12 deletions(-) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index bfdc35336..f8fe221cb 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -38,7 +38,6 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp - src/absolute_pose_3d_stamped_euler_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -52,7 +51,6 @@ add_library(${PROJECT_NAME} src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp - src/relative_pose_3d_stamped_euler_constraint.cpp src/uuid_ordering.cpp src/variable_constraints.cpp ) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index b4c4bd7c6..6dee9a605 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -87,11 +87,6 @@ A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose in quat. - - - A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose in rpy. - - A constraint that represents remaining marginal information on a set of variables. @@ -147,9 +142,4 @@ A constraint that represents a measurement on the difference between two 3D poses. - - - A constraint that represents a measurement on the difference between two 3D poses in rpy. - - From d846fff7d3c31eb179430d274a812e2332dbfeef Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 8 Nov 2023 17:18:05 +0000 Subject: [PATCH 014/116] Add support partial measurements for odometry 3D --- .../fuse_models/common/sensor_config.hpp | 6 +- .../fuse_models/common/sensor_proc.hpp | 439 ++++++++++++------ .../include/fuse_models/unicycle_3d.hpp | 12 +- .../unicycle_3d_state_cost_functor.hpp | 33 +- fuse_models/src/odometry_3d.cpp | 3 - fuse_models/src/odometry_3d_publisher.cpp | 6 +- fuse_models/src/unicycle_3d.cpp | 9 +- ...unicycle_3d_state_kinematic_constraint.cpp | 2 +- 8 files changed, 331 insertions(+), 179 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index 8c8ed84de..6967ec6b3 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -152,13 +152,13 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "roll" || lower_dim == "x") { - return static_cast(T::Euler::ROLL) - 1UL; + return static_cast(T::Euler::ROLL) - 4UL; } if (lower_dim == "pitch" || lower_dim == "y") { - return static_cast(T::Euler::PITCH) - 1UL; + return static_cast(T::Euler::PITCH) - 4UL; } if (lower_dim == "yaw" || lower_dim == "z") { - return static_cast(T::Euler::YAW) - 1UL; + return static_cast(T::Euler::YAW) - 4UL; } throwDimensionError(dimension); diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index b926fc27c..acee93e63 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -57,10 +57,15 @@ #include #include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include @@ -218,9 +223,6 @@ inline void populatePartialMeasurement( fuse_core::MatrixXd & covariance_partial) { covariance_partial.setZero(); - // for (auto & i : indices) { - // covariance_partial.col(i) = covariance_full.col(i); - // } for (size_t r = 0; r < indices.size(); ++r) { for (size_t c = 0; c < indices.size(); ++c) { covariance_partial(r, c) = covariance_full(indices[r], indices[c]); @@ -499,36 +501,54 @@ inline bool processAbsolutePose3DWithCovariance( } } + // Convert the ROS message into tf2 transform + tf2::Transform tf2_pose; + tf2::fromMsg(transformed_message.pose.pose, tf2_pose); + + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_mean; + pose_mean(0) = tf2_pose.getOrigin().x(); + pose_mean(1) = tf2_pose.getOrigin().y(); + pose_mean(2) = tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( + pose_mean(3), pose_mean(4), pose_mean(5)); + + Eigen::Map pose_covariance_map(transformed_message.pose.covariance.data()); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_mean.data(), pose_mean.data() + pose_mean.size(), + [&indices, &pose_mean](const double & value) { + return std::find(indices.begin(), indices.end(), &value - pose_mean.data()) == indices.end(); + }, 0.0); + + fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement(pose_covariance_map, indices, pose_covariance_partial); + + tf2::Quaternion q_partial; + q_partial.setRPY(pose_mean(3), pose_mean(4), pose_mean(5)); + // Create the pose variable auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); - position->x() = pose.pose.pose.position.x; - position->y() = pose.pose.pose.position.y; - position->z() = pose.pose.pose.position.z; - orientation->x() = pose.pose.pose.orientation.x; - orientation->y() = pose.pose.pose.orientation.y; - orientation->z() = pose.pose.pose.orientation.z; - orientation->w() = pose.pose.pose.orientation.w; - - - // Create the pose for the constraint - fuse_core::Vector7d pose_mean; - pose_mean << pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.position.z, - pose.pose.pose.orientation.w, pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z; - - // Create the covariance for the constraint - Eigen::Map cov_map(pose.pose.covariance.data()); - fuse_core::Matrix6d pose_covariance = cov_map; // TODO check how to avoid this to not doing copies - - const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); - fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); - populatePartialMeasurement(pose_covariance, indices, pose_covariance_partial); + position->x() = pose_mean(0); + position->y() = pose_mean(1); + position->z() = pose_mean(2); + orientation->x() = q_partial.getX(); + orientation->y() = q_partial.getY(); + orientation->z() = q_partial.getZ(); + orientation->w() = q_partial.getW(); + // Create the pose for the constraint + fuse_core::Vector7d pose_mean_partial; + pose_mean_partial << pose_mean(0), pose_mean(1), pose_mean(2), + q_partial.getW(), q_partial.getX(), q_partial.getY(), q_partial.getZ(); + if (validate) { try { - validatePartialMeasurement(pose_mean, pose_covariance_partial, 1e-5); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -537,27 +557,15 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - std::cout << "AbsolutePose3DStampedEulerConstraint creating..." << std::endl; - - // Create an absolute pose constraint - auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( + + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( source, *position, *orientation, - pose_mean, + pose_mean_partial, pose_covariance_partial, - position_indices, - orientation_indices); - - // auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( - // source, - // *position, - // *orientation, - // pose_mean, - // pose_covariance); + indices); - std::cout << "AbsolutePose3DStampedEulerConstraint created" << std::endl; - constraint->loss(loss); transaction.addVariable(position); @@ -974,117 +982,141 @@ inline bool processDifferentialPose3DWithCovariance( const bool validate, fuse_core::Transaction & transaction) { - // Better to convert right now in covariance_geometry types, since we need to compute relative poses - // and covariances. + // Pipeline to consider only the measured positions and orientations + // Convert the ROS message into tf2 transform + // Fill eigen pose in RPY representation + // Set the components which are not measured to zero + // Create the pose variable for the graph + // Create the pose for the constraint (in quaternion) + // Create the constraint + + // Convert from ROS msg to covariance geometry types // PoseQuaternionCovarianceRPY is std::pair, Covariance> // Position is Eigen::Vector3d // Quaternion is Eigen::Quaterniond // Covariance is Eigen::Matrix6d - - covariance_geometry::PoseQuaternionCovarianceRPY pose1_3d, pose2_3d; - covariance_geometry::fromROS(pose1.pose, pose1_3d); - covariance_geometry::fromROS(pose2.pose, pose2_3d); - + // TODO: implement a fromROS method which outputs PoseRPYCovariance directly + covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::fromROS(pose1.pose, p1); + covariance_geometry::fromROS(pose2.pose, p2); + + // Create the delta for the constraint and the relative covariance + covariance_geometry::ComposePoseQuaternionCovarianceRPY( + covariance_geometry::inversePose3DQuaternionCovarianceRPY(p1), + p2, + p12); + + // Convert the poses into RPY representation + covariance_geometry::PoseRPYCovariance p1_rpy, p2_rpy, p12_rpy; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( + p1, p1_rpy); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( + p2, p2_rpy); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( + p12, p12_rpy); + + // Set the components which are not measured to zero + // p1_partial + std::replace_if( + p1_rpy.first.first.data(), p1_rpy.first.first.data() + p1_rpy.first.first.size(), + [&position_indices, &p1_rpy](const double & value) { + return std::find( + position_indices.begin(), + position_indices.end(), + &value - p1_rpy.first.first.data()) == position_indices.end(); + }, 0.0); + + std::replace_if( + p1_rpy.first.second.data(), p1_rpy.first.second.data() + p1_rpy.first.second.size(), + [&orientation_indices, &p1_rpy](const double & value) { + return std::find( + orientation_indices.begin(), + orientation_indices.end(), + &value - p1_rpy.first.second.data()) == orientation_indices.end(); + }, 0.0); + + // p2_partial + std::replace_if( + p2_rpy.first.first.data(), p2_rpy.first.first.data() + p2_rpy.first.first.size(), + [&position_indices, &p2_rpy](const double & value) { + return std::find( + position_indices.begin(), + position_indices.end(), + &value - p2_rpy.first.first.data()) == position_indices.end(); + }, 0.0); + + std::replace_if( + p2_rpy.first.second.data(), p2_rpy.first.second.data() + p2_rpy.first.second.size(), + [&orientation_indices, &p2_rpy](const double & value) { + return std::find( + orientation_indices.begin(), + orientation_indices.end(), + &value - p2_rpy.first.second.data()) == orientation_indices.end(); + }, 0.0); + + // p12_partial + std::replace_if( + p12_rpy.first.first.data(), p12_rpy.first.first.data() + p12_rpy.first.first.size(), + [&position_indices, &p12_rpy](const double & value) { + return std::find( + position_indices.begin(), + position_indices.end(), + &value - p12_rpy.first.first.data()) == position_indices.end(); + }, 0.0); + std::replace_if( + p12_rpy.first.second.data(), p12_rpy.first.second.data() + p12_rpy.first.second.size(), + [&orientation_indices, &p12_rpy](const double & value) { + return std::find( + orientation_indices.begin(), + orientation_indices.end(), + &value - p12_rpy.first.second.data()) == orientation_indices.end(); + }, 0.0); + + // Convert back to quaternion + covariance_geometry::PoseQuaternionCovarianceRPY p1_partial, p2_partial, p12_partial; + covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( + p1_rpy, p1_partial); + covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( + p2_rpy, p2_partial); + covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( + p12_rpy, p12_partial); + // Create the pose variables auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); - position1->x() = pose1_3d.first.first.x(); - position1->y() = pose1_3d.first.first.y(); - position1->z() = pose1_3d.first.first.z(); - orientation1->x() = pose1_3d.first.second.x(); - orientation1->y() = pose1_3d.first.second.y(); - orientation1->z() = pose1_3d.first.second.z(); - orientation1->w() = pose1_3d.first.second.w(); + position1->x() = p1_partial.first.first.x(); + position1->y() = p1_partial.first.first.y(); + position1->z() = p1_partial.first.first.z(); + orientation1->x() = p1_partial.first.second.x(); + orientation1->y() = p1_partial.first.second.y(); + orientation1->z() = p1_partial.first.second.z(); + orientation1->w() = p1_partial.first.second.w(); auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); - position2->x() = pose2_3d.first.first.x(); - position2->y() = pose2_3d.first.first.y(); - position2->z() = pose2_3d.first.first.z(); - orientation2->x() = pose2_3d.first.second.x(); - orientation2->y() = pose2_3d.first.second.y(); - orientation2->z() = pose2_3d.first.second.z(); - orientation2->w() = pose2_3d.first.second.w(); - - // Create the delta for the constraint and the relative covariance - // Technically there should be no differences between dependent and independent poses - covariance_geometry::PoseQuaternionCovarianceRPY pose_relative_3d, pose1_inv_3d; - pose1_inv_3d = covariance_geometry::inversePose3DQuaternionCovarianceRPY(pose1_3d); - covariance_geometry::ComposePoseQuaternionCovarianceRPY( - pose1_inv_3d, pose2_3d, pose_relative_3d); - - // if (orientation_indices.size() < 3u) { - // // We don't use all the orientations, so we switch to rpy representation - // covariance_geometry::PoseRPYCovariance pose_relative_3d_rpy; - // covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( - // pose_relative_3d, pose_relative_3d_rpy); - - // fuse_core::Vector6d pose_relative_mean; - // pose_relative_mean << pose_relative_3d_rpy.first.first, pose_relative_3d_rpy.first.second; - // fuse_core::Matrix6d pose_relative_covariance; - // pose_relative_covariance = pose_relative_3d_rpy.second + minimum_pose_relative_covariance; - - // // Build the sub-vector and sub-matrices based on the requested indices - // fuse_core::VectorXd pose_relative_mean_partial( - // position_indices.size() + orientation_indices.size()); - // fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), - // pose_relative_mean_partial.rows()); - - // const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); - - // populatePartialMeasurement( - // pose_relative_mean, - // pose_relative_covariance, - // indices, - // pose_relative_mean_partial, - // pose_relative_covariance_partial); - - // if (validate) { - // try { - // validatePartialMeasurement( - // pose_relative_mean_partial, pose_relative_covariance_partial, - // 1e-6); - // } catch (const std::runtime_error & ex) { - // RCLCPP_ERROR_STREAM_THROTTLE( - // rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - // "Invalid partial differential pose measurement from '" - // << source << "' source: " << ex.what()); - // return false; - // } - // } - // // Create a relative pose constraint. - // // TODO: implement relative pose constraint with orientation in rpy - // auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( - // source, - // *position1, - // *orientation1, - // *position2, - // *orientation2, - // pose_relative_mean, - // pose_relative_covariance); - - // constraint->loss(loss); - - // transaction.addVariable(position1); - // transaction.addVariable(orientation1); - // transaction.addVariable(position2); - // transaction.addVariable(orientation2); - // transaction.addConstraint(constraint); - // transaction.addInvolvedStamp(pose1.header.stamp); - // transaction.addInvolvedStamp(pose2.header.stamp); - - // return true; - // } + position2->x() = p2_partial.first.first.x(); + position2->y() = p2_partial.first.first.y(); + position2->z() = p2_partial.first.first.z(); + orientation2->x() = p2_partial.first.second.x(); + orientation2->y() = p2_partial.first.second.y(); + orientation2->z() = p2_partial.first.second.z(); + orientation2->w() = p2_partial.first.second.w(); + // Create mean vector and partial covariance matrix for the constraint fuse_core::Vector7d pose_relative_mean; - pose_relative_mean << pose_relative_3d.first.first.x(), pose_relative_3d.first.first.y(), - pose_relative_3d.first.first.z(), pose_relative_3d.first.second.w(), - pose_relative_3d.first.second.x(), pose_relative_3d.first.second.y(), - pose_relative_3d.first.second.z(); - fuse_core::Matrix6d pose_relative_covariance; - pose_relative_covariance = pose_relative_3d.second + minimum_pose_relative_covariance; + pose_relative_mean << + p12_partial.first.first.x(), p12_partial.first.first.y(), p12_partial.first.first.z(), + p12_partial.first.second.w(), p12_partial.first.second.x(), p12_partial.first.second.y(), + p12_partial.first.second.z(); + + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + fuse_core::MatrixXd pose_relative_covariance(indices.size(), indices.size()); + populatePartialMeasurement( + p12_partial.second + minimum_pose_relative_covariance, + indices, + pose_relative_covariance); if (validate) { try { @@ -1099,7 +1131,7 @@ inline bool processDifferentialPose3DWithCovariance( } } // Create a relative pose constraint. - auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( source, *position1, *orientation1, @@ -1107,8 +1139,7 @@ inline bool processDifferentialPose3DWithCovariance( *orientation2, pose_relative_mean, pose_relative_covariance, - position_indices, - orientation_indices); + indices); constraint->loss(loss); @@ -1567,9 +1598,9 @@ inline bool processTwist3DWithCovariance( // Create the covariance for the constraint // TODO check if this the correct way for not doing copies - Eigen::Map cov_map( + Eigen::Map linear_vel_covariance_map( transformed_message.twist.covariance.data()) ; - fuse_core::Matrix3d linear_vel_covariance = cov_map.block<3, 3>(0, 0); + // fuse_core::Matrix3d linear_vel_covariance = cov_map.block<3, 3>(0, 0); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); @@ -1578,7 +1609,7 @@ inline bool processTwist3DWithCovariance( populatePartialMeasurement( linear_vel_mean, - linear_vel_covariance, + linear_vel_covariance_map.block<3, 3>(0, 0), linear_indices, linear_vel_mean_partial, linear_vel_covariance_partial); @@ -1625,9 +1656,8 @@ inline bool processTwist3DWithCovariance( transformed_message.twist.twist.angular.z; // Create the covariance for the constraint - // TODO check if this the correct way for not doing copies - Eigen::Map cov_map(transformed_message.twist.covariance.data()); - fuse_core::Matrix3d angular_vel_covariance = cov_map.block<3,3>(3,3); + Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance.data()); + // fuse_core::Matrix3d angular_vel_covariance = cov_map.block<3,3>(3,3); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); @@ -1636,7 +1666,7 @@ inline bool processTwist3DWithCovariance( populatePartialMeasurement( angular_vel_mean, - angular_vel_covariance, + angular_vel_cov_map.block<3, 3>(3, 3), angular_indices, angular_vel_mean_partial, angular_vel_covariance_partial); @@ -1786,6 +1816,117 @@ inline bool processAccelWithCovariance( return true; } +/** + * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to + * a fuse Transaction + * + * This method effectively adds a linear acceleration variable and constraint to the given to the + * given \p transaction. The acceleration data is extracted from the \p acceleration message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] acceleration - The AccelWithCovarianceStamped message from which we will extract the + * acceleration data + * @param[in] loss - The loss function for the 3D linear acceleration constraint generated + * @param[in] indices - The indices of the linear acceleration vector to use. If empty, no + * linear acceleration constraint is added + * @param[in] target_frame - The frame ID into which the acceleration data will be transformed + * before it is used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAccel3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::AccelWithCovarianceStamped & acceleration, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (indices.empty()) { + return false; + } + + geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = acceleration; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Failed to transform acceleration message with stamp " << + rclcpp::Time(acceleration.header.stamp).nanoseconds() + << ". Cannot create constraint."); + return false; + } + } + + // Create the full mean vector and covariance for the constraint + fuse_core::Vector3d accel_mean; + accel_mean << + transformed_message.accel.accel.linear.x, + transformed_message.accel.accel.linear.y, + transformed_message.accel.accel.linear.z; + + Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd accel_mean_partial(indices.size()); + fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), + accel_mean_partial.rows()); + + populatePartialMeasurement( + accel_mean, accel_covariance_map, indices, accel_mean_partial, + accel_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create the acceleration variables + auto acceleration_linear = + fuse_variables::AccelerationLinear3DStamped::make_shared(acceleration.header.stamp, device_id); + acceleration_linear->x() = transformed_message.accel.accel.linear.x; + acceleration_linear->y() = transformed_message.accel.accel.linear.y; + acceleration_linear->z() = transformed_message.accel.accel.linear.z; + + // Create the constraint + auto linear_accel_constraint = + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + source, + *acceleration_linear, + accel_mean_partial, + accel_covariance_partial, + indices); + + linear_accel_constraint->loss(loss); + + transaction.addVariable(acceleration_linear); + transaction.addConstraint(linear_accel_constraint); + transaction.addInvolvedStamp(acceleration.header.stamp); + + return true; +} + /** * @brief Scales the process noise covariance pose by the norm of the velocity * diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/unicycle_3d.hpp index 7d12e30cc..cd2acbac0 100644 --- a/fuse_models/include/fuse_models/unicycle_3d.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d.hpp @@ -108,13 +108,11 @@ class Unicycle3D : public fuse_core::AsyncMotionModel fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration //!< variable - - fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position - // fuse_core::Vector3d orientation; //!< Map-frame orientation (roll, pitch, yaw) - fuse_core::Quaternion orientation = {1.0, 0.0, 0.0, 0.0}; //!< Map-frame orientation (quaternion) - fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities - fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities - fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position + fuse_core::Quaternion orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations void print(std::ostream & stream = std::cout) const; diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index f467685ba..056dfadbc 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -34,7 +34,6 @@ #ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ #define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ -// #include #include #include @@ -167,16 +166,36 @@ bool Unicycle3DStateCostFunctor::operator()( acc_linear_pred); Eigen::Map> residuals_map(residual); - Eigen::Map> q_pred(orientation_pred), q2(orientation2); - Eigen::Quaternion q_res = q2.inverse() * q_pred; - + // Eigen::Map> q_pred(orientation_pred), q2(orientation2); + // Eigen::Quaternion q_res = q2.inverse() * q_pred; + + T orientation_pred_rpy[3]; + T orientation2_rpy[3]; + + orientation_pred_rpy[0] = + fuse_core::getRoll(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); + orientation_pred_rpy[1] = + fuse_core::getPitch(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); + orientation_pred_rpy[2] = + fuse_core::getYaw(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); + + orientation2_rpy[0] = + fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + orientation2_rpy[1] = + fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + orientation2_rpy[2] = + fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + residuals_map(0) = T(position2[0] - position_pred[0]); residuals_map(1) = T(position2[1] - position_pred[1]); residuals_map(2) = T(position2[2] - position_pred[2]); - residuals_map(3) = T(q_res.x()); - residuals_map(4) = T(q_res.y()); - residuals_map(5) = T(q_res.z()); + // residuals_map(3) = T(q_res.x()); + // residuals_map(4) = T(q_res.y()); + // residuals_map(5) = T(q_res.z()); // residuals_map(6) = T(q_res.w()); + residuals_map(3) = T(orientation2_rpy[0] - orientation_pred_rpy[0]); + residuals_map(4) = T(orientation2_rpy[1] - orientation_pred_rpy[1]); + residuals_map(5) = T(orientation2_rpy[2] - orientation_pred_rpy[2]); residuals_map(6) = T(vel_linear2[0] - vel_linear_pred[0]); residuals_map(7) = T(vel_linear2[1] - vel_linear_pred[1]); residuals_map(8) = T(vel_linear2[2] - vel_linear_pred[2]); diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 7389c8c6f..493b5a152 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -65,7 +65,6 @@ void Odometry3D::initialize( { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); - std::cout << "Odometry3D initialized" << std::endl; } void Odometry3D::onInit() @@ -136,7 +135,6 @@ void Odometry3D::onStop() void Odometry3D::process(const nav_msgs::msg::Odometry & msg) { // Create a transaction object - std::cout << "Odometry3D process" << std::endl; auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); @@ -184,7 +182,6 @@ void Odometry3D::process(const nav_msgs::msg::Odometry & msg) *transaction, params_.tf_timeout); - transaction->print(std::cout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 7d0a61333..89fff0a18 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -198,7 +198,7 @@ void Odometry3DPublisher::notifyCallback( covariance_requests.emplace_back(acceleration_linear_uuid, acceleration_linear_uuid); std::vector> covariance_matrices; - graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options); + graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options, true); odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) @@ -407,7 +407,7 @@ bool Odometry3DPublisher::getState( auto acceleration_linear_variable = dynamic_cast( graph.getVariable(acceleration_linear_uuid)); - + odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = position_variable.z(); @@ -469,8 +469,6 @@ void Odometry3DPublisher::publishTimerCallback() tf2::Transform pose; tf2::fromMsg(odom_output.pose.pose, pose); - // covariance_geometry::PoseQuaternionCovarianceRPY pose; - // covariance_geometry::fromROS(odom_output.pose, pose); // If requested, we need to project our state forward in time using the 3D kinematic model if (params_.predict_to_current_time) { diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index d5c7dfc09..f7f358f91 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -70,10 +70,10 @@ inline bool isfinite(const fuse_core::Vector3d & vector) return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); } -inline bool isfinite(const fuse_core::Quaternion & quaternion) +inline bool isNormalized(const fuse_core::Quaternion & q) { - return std::isfinite(quaternion.x()) && std::isfinite(quaternion.y()) && - std::isfinite(quaternion.z()) && std::isfinite(quaternion.w()); + return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < + Eigen::NumTraits::dummy_precision(); } std::string to_string(const fuse_core::Vector3d & vector) @@ -155,7 +155,7 @@ void Unicycle3D::StateHistoryElement::validate() const if (!std::isfinite(position)) { throw std::runtime_error("Invalid position " + std::to_string(position)); } - if (!std::isfinite(orientation)) { + if (!std::isNormalized(orientation)) { throw std::runtime_error("Invalid orientation " + std::to_string(orientation)); } if (!std::isfinite(vel_linear)) { @@ -352,7 +352,6 @@ void Unicycle3D::generateMotionModel( state2.vel_angular, state2.acc_linear); - // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp index 8727eba47..675240059 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -63,7 +63,7 @@ Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( const fuse_variables::VelocityAngular3DStamped & velocity_angular2, const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, const fuse_core::Matrix15d & covariance) -: fuse_core::Constraint( // TODO: check if there is a constructor for these arguments +: fuse_core::Constraint( source, {position1.uuid(), orientation1.uuid(), From 052be965a31532814a3210de051c89fd44a4accb Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 8 Nov 2023 17:18:36 +0000 Subject: [PATCH 015/116] imu 3D plugin dev --- fuse_models/CMakeLists.txt | 1 + fuse_models/fuse_plugins.xml | 6 + fuse_models/include/fuse_models/imu_3d.hpp | 194 +++++++++++ .../fuse_models/parameters/imu_3d_params.hpp | 215 ++++++++++++ fuse_models/src/imu_3d.cpp | 318 ++++++++++++++++++ 5 files changed, 734 insertions(+) create mode 100644 fuse_models/include/fuse_models/imu_3d.hpp create mode 100644 fuse_models/include/fuse_models/parameters/imu_3d_params.hpp create mode 100644 fuse_models/src/imu_3d.cpp diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 12509630f..dba1babd4 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -44,6 +44,7 @@ add_library(${PROJECT_NAME} src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp + src/imu_3d.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp src/odometry_3d.cpp diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 4c3120686..4a8003d34 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -61,6 +61,12 @@ acceleration constraints from IMU sensor data published by another node + + + An adapter-type sensor that produces 3D orientation (relative or absolute), angular velocity, and linear + acceleration constraints from IMU sensor data published by another node + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp new file mode 100644 index 000000000..2480f466e --- /dev/null +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_MODELS__IMU_3D_HPP_ +#define FUSE_MODELS__IMU_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces orientation (relative or absolute), angular velocity, + * and linear acceleration constraints from IMU sensor data published by another node + * + * This sensor subscribes to a sensor_msgs::msg::Imu topic and: + * 1. Creates relative or absolute orientation and constraints. If the \p differential parameter + * is set to false (the default), the orientation measurement will be treated as an absolute + * constraint. If it is set to true, consecutive measurements will be used to generate relative + * orientation constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the orientation, angular velocity, and linear acceleration + * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D + * classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse orientation measurements + * absolutely, or to create relative orientation constraints + * using consecutive measurements. + * - remove_gravitational_acceleration (bool, default: false) Whether we should remove acceleration + * due to gravity from the acceleration + * values produced by the IMU before + * fusing + * - gravitational_acceleration (double, default: 9.80665) Acceleration due to gravity, in + * meters/sec^2. This value is only used if + * \p remove_gravitational_acceleration is + * true + * - orientation_target_frame (string) Orientation data will be transformed into this frame before + * it is fused. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. + * - acceleration_target_frame (string) Acceleration data will be transformed into this frame + * before it is fused. + * + * Subscribes: + * - \p topic (sensor_msgs::msg::Imu) IMU data at a given timestep + */ +class Imu3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Imu3D) + using ParameterType = parameters::Imu3DParams; + + /** + * @brief Default constructor + */ + Imu3D(); + + /** + * @brief Destructor + */ + virtual ~Imu3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The IMU message to process + */ + void process(const sensor_msgs::msg::Imu & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; + ImuThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__IMU_3D_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp new file mode 100644 index 000000000..2fc3b2b25 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Imu3D class + */ +struct Imu3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + linear_acceleration_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_acceleration_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam(interfaces, "tf_timeout", tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, "throttle_period", throttle_period, false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); + gravitational_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "gravitational_acceleration"), + gravitational_acceleration); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + if (differential) { + // TODO: understand if this is needed + // independent = fuse_core::getParam( + // interfaces, fuse_core::joinParameterName( + // ns, + // "independent"), + // independent); + // use_twist_covariance = + // fuse_core::getParam( + // interfaces, fuse_core::joinParameterName( + // ns, + // "use_twist_covariance"), + // use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + acceleration_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_target_frame"), + acceleration_target_frame); + orientation_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_target_frame"), + orientation_target_frame); + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + linear_acceleration_loss = + fuse_core::loadLossConfig( + interfaces, + fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration {false}; + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration {9.80665}; + std::string acceleration_target_frame {}; + std::string orientation_target_frame {}; + std::string topic {}; + std::string twist_target_frame {}; + std::vector angular_velocity_indices; + std::vector linear_acceleration_indices; + std::vector orientation_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; + fuse_core::Loss::SharedPtr linear_acceleration_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp new file mode 100644 index 000000000..e3b2eed72 --- /dev/null +++ b/fuse_models/src/imu_3d.cpp @@ -0,0 +1,318 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Imu3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Imu3D::Imu3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) +{ +} + +void Imu3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Imu3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.orientation_indices.empty() && + params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Imu3D::onStart() +{ + if (!params_.orientation_indices.empty() || + !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &ImuThrottledCallback::callback, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Imu3D::onStop() +{ + sub_.reset(); +} + +void Imu3D::process(const sensor_msgs::msg::Imu & msg) +{ + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the orientation data (treat it as a pose, but with only orientation indices used) + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose.pose.orientation = msg.orientation; + pose->pose.covariance[21] = msg.orientation_covariance[0]; + pose->pose.covariance[22] = msg.orientation_covariance[1]; + pose->pose.covariance[23] = msg.orientation_covariance[2]; + pose->pose.covariance[27] = msg.orientation_covariance[3]; + pose->pose.covariance[28] = msg.orientation_covariance[4]; + pose->pose.covariance[29] = msg.orientation_covariance[5]; + pose->pose.covariance[33] = msg.orientation_covariance[6]; + pose->pose.covariance[34] = msg.orientation_covariance[7]; + pose->pose.covariance[35] = msg.orientation_covariance[8]; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.twist.twist.angular = msg.angular_velocity; + twist.twist.covariance[21] = msg.angular_velocity_covariance[0]; + twist.twist.covariance[22] = msg.angular_velocity_covariance[1]; + twist.twist.covariance[23] = msg.angular_velocity_covariance[2]; + twist.twist.covariance[27] = msg.angular_velocity_covariance[3]; + twist.twist.covariance[28] = msg.angular_velocity_covariance[4]; + twist.twist.covariance[29] = msg.angular_velocity_covariance[5]; + twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; + twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; + twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.orientation_target_frame, + {}, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data (only include indices for angular velocity) + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + nullptr, + params_.angular_velocity_loss, + params_.twist_target_frame, + {}, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Handle the acceleration data + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.header = msg.header; + accel.accel.accel.linear = msg.linear_acceleration; + accel.accel.covariance[0] = msg.linear_acceleration_covariance[0]; + accel.accel.covariance[1] = msg.linear_acceleration_covariance[1]; + accel.accel.covariance[2] = msg.linear_acceleration_covariance[2]; + accel.accel.covariance[6] = msg.linear_acceleration_covariance[3]; + accel.accel.covariance[7] = msg.linear_acceleration_covariance[4]; + accel.accel.covariance[8] = msg.linear_acceleration_covariance[5]; + accel.accel.covariance[12] = msg.linear_acceleration_covariance[6]; + accel.accel.covariance[13] = msg.linear_acceleration_covariance[7]; + accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; + + // Optionally remove the acceleration due to gravity + if (params_.remove_gravitational_acceleration) { + geometry_msgs::msg::Vector3 accel_gravity; + accel_gravity.z = params_.gravitational_acceleration; + geometry_msgs::msg::TransformStamped orientation_trans; + tf2::Quaternion imu_orientation; + tf2::fromMsg(msg.orientation, imu_orientation); + orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse()); + tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp + accel.accel.accel.linear.x -= accel_gravity.x; + accel.accel.accel.linear.y -= accel_gravity.y; + accel.accel.accel.linear.z -= accel_gravity.z; + } + + common::processAccel3DWithCovariance( + name(), + device_id_, + accel, + params_.linear_acceleration_loss, + params_.acceleration_target_frame, + params_.linear_acceleration_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Imu3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. + orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() + << " to orientation target frame " << + params_.orientation_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + // if (params_.use_twist_covariance) { + // geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + // transformed_twist.header.frame_id = + // params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + // if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + // RCLCPP_WARN_STREAM_THROTTLE( + // logger_, *clock_, 5.0 * 1000, + // "Cannot transform twist message with stamp " << rclcpp::Time( + // twist.header.stamp).nanoseconds() + // << " to twist target frame " << + // params_.twist_target_frame); + // } else { + // common::processDifferentialPoseWithTwistCovariance( + // name(), + // device_id_, + // *previous_pose_, + // *transformed_pose, + // twist, + // params_.minimum_pose_relative_covariance, + // params_.twist_covariance_offset, + // params_.pose_loss, + // {}, + // params_.orientation_indices, + // validate, + // transaction); + // } + // } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + // } + + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models From ac1a0f853c10719e6c6e91e207673ce7cb3d6489 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 14 Nov 2023 08:43:53 +0000 Subject: [PATCH 016/116] fix missing type() template specialization for vel 3d --- .../fuse_constraints/absolute_constraint_impl.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..e027efb48 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -185,6 +185,18 @@ inline std::string AbsoluteConstraint:: return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; } +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint"; +} + } // namespace fuse_constraints #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_ From c51617640854da4a40f9fa09a500d0d341ce7cd0 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 15 Nov 2023 15:42:52 +0000 Subject: [PATCH 017/116] add support for partial measurements in absolute_orientation_3d_stamped_constraint --- ...lute_orientation_3d_stamped_constraint.hpp | 20 +++++++- ...rmal_prior_orientation_3d_cost_functor.hpp | 16 ++++-- ...lute_orientation_3d_stamped_constraint.cpp | 49 +++++++++++++++++-- 3 files changed, 74 insertions(+), 11 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp index 0f63bdf86..5e025208c 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp @@ -85,6 +85,22 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector4d & mean, const fuse_core::Matrix3d & covariance); + + /** + * @brief Create a constraint using a measurement/prior of a 3D orientation + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior orientation as a quaternion (4x1 vector: w, x, y, z) + * @param[in] partial_covariance The measurement/prior covariance (max 3x3 matrix: qx, qy, qz) + * @param[in] orientation_indices The indices of the measured variables + */ + AbsoluteOrientation3DStampedConstraint( + const std::string & source, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector4d & mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & orientation_indices); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -131,7 +147,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z) */ - const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. @@ -182,7 +198,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint static fuse_core::Matrix3d toEigen(const std::array & covariance); fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable - fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: // Allow Boost Serialization access to private methods diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp index b2702ee2d..5b817731d 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -80,11 +81,13 @@ class NormalPriorOrientation3DCostFunctor * @param[in] b The orientation measurement or prior in order (w, x, y, z) */ NormalPriorOrientation3DCostFunctor( - const fuse_core::Matrix3d & A, + const fuse_core::MatrixXd & A, const fuse_core::Vector4d & b) : A_(A), b_(b) { + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 3); } /** @@ -96,6 +99,8 @@ class NormalPriorOrientation3DCostFunctor using fuse_variables::Orientation3DStamped; // Compute the delta quaternion + T full_residuals[3]; + T variable[4] = { orientation[0], @@ -114,18 +119,19 @@ class NormalPriorOrientation3DCostFunctor T difference[4]; ceres::QuaternionProduct(observation_inverse, variable, difference); - ceres::QuaternionToAngleAxis(difference, residuals); + ceres::QuaternionToAngleAxis(difference, full_residuals); // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map> residuals_map(residuals); - residuals_map.applyOnTheLeft(A_.template cast()); + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; return true; } private: - fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root //!< information matrix fuse_core::Vector4d b_; //!< The measured 3D orientation (quaternion) value }; diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 0131bfd20..c6d0c8e1e 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -55,6 +55,34 @@ AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( { } +AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( + const std::string & source, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector4d & mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & orientation_indices) +: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) + mean_(mean) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(orientation_indices.size(), 3); + for (size_t i = 0; i < orientation_indices.size(); ++i) + { + sqrt_information_.col(orientation_indices[i]) = partial_sqrt_information.col(i); + } +} + AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( const std::string & source, const fuse_variables::Orientation3DStamped & orientation, @@ -75,8 +103,20 @@ AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::covariance() const { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); -} + if (sqrt_information_.rows() == 3) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose();} void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const { @@ -95,8 +135,9 @@ void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsoluteOrientation3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); + return new ceres::AutoDiffCostFunction( + new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_), + sqrt_information_.rows()); } fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( From 16f4c535b19b31e5cf34cb6f4c617d6e61fbec96 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 15 Nov 2023 15:53:12 +0000 Subject: [PATCH 018/116] fix: missing tf_timeout param --- fuse_models/src/imu_3d.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index e3b2eed72..46f4f5945 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -167,7 +167,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - + const bool validate = !params_.disable_checks; if (params_.differential) { @@ -256,7 +256,7 @@ void Imu3D::processDifferential( params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. orientation_target_frame; - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { RCLCPP_WARN_STREAM_THROTTLE( logger_, *clock_, 5.0 * 1000, "Cannot transform pose message with stamp " << rclcpp::Time( @@ -266,9 +266,9 @@ void Imu3D::processDifferential( return; } - if (!previous_pose_) { + if (!previous_pose_) { previous_pose_ = std::move(transformed_pose); - return; + return; } // if (params_.use_twist_covariance) { From 5d4dd92a60fb285b3e90b7b5cac2bf17bd22ffeb Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 15 Nov 2023 15:53:56 +0000 Subject: [PATCH 019/116] removed euler constraint headers --- fuse_models/include/fuse_models/common/sensor_proc.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index acee93e63..9e0f6a7b5 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -47,10 +47,8 @@ #include #include -#include #include #include -#include #include #include #include @@ -517,7 +515,7 @@ inline bool processAbsolutePose3DWithCovariance( // Set the components which are not measured to zero const auto indices = mergeIndices(position_indices, orientation_indices, 3); - std::replace_if( + std::replace_if( pose_mean.data(), pose_mean.data() + pose_mean.size(), [&indices, &pose_mean](const double & value) { return std::find(indices.begin(), indices.end(), &value - pose_mean.data()) == indices.end(); @@ -557,7 +555,7 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( source, *position, From 0c63c4509edf7217aee26ccb2fa5ccff4d8f75bb Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 15 Nov 2023 15:56:04 +0000 Subject: [PATCH 020/116] fix: wrong array mapping in unicycle 3d motion model + tests --- .../fuse_models/unicycle_3d_predict.hpp | 336 ++--------- .../unicycle_3d_predict_no_vec.hpp | 311 ---------- .../unicycle_3d_state_cost_functor.hpp | 14 +- fuse_models/test/CMakeLists.txt | 3 +- fuse_models/test/test_unicycle_3d_predict.cpp | 545 ++++++++++++++++-- .../test_unicycle_3d_state_cost_function.cpp | 61 +- 6 files changed, 597 insertions(+), 673 deletions(-) delete mode 100644 fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index 844c1eb9c..c819fd9e2 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -43,102 +43,7 @@ namespace fuse_models { - using Jet = ceres::Jet; -// /** -// * @brief Given a state and time delta, predicts a new state - -// * inner templated version for ceres autodiff -// * @param[in] position1 - First position -// * @param[in] orientation1 - First orientation -// * @param[in] vel_linear1 - First linear velocity -// * @param[in] vel_angular1 - First angular velocity -// * @param[in] acc_linear1 - First linear acceleration -// * @param[in] dt - The time delta across which to predict the state -// * @param[out] position2 - Second position -// * @param[out] orientation2 - Second orientation -// * @param[out] vel_linear2 - Second linear velocity -// * @param[out] vel_angular2 - Second angular velocity -// * @param[out] acc_linear2 - Second linear acceleration -// */ -// template -// inline void predict_eigen( -// // const Eigen::Ref>& position1, -// // const Eigen::Ref>& orientation1, -// // const Eigen::Ref>& vel_linear1, -// // const Eigen::Ref>& vel_angular1, -// // const Eigen::Ref>& acc_linear1, -// // const T dt, -// // Eigen::Ref> position2, -// // Eigen::Ref> orientation2, -// // Eigen::Ref> vel_linear2, -// // Eigen::Ref> vel_angular2, -// // Eigen::Ref> acc_linear2) -// const Eigen::Matrix& position1, -// const Eigen::Quaternion& orientation1, -// const Eigen::Matrix& vel_linear1, -// const Eigen::Matrix& vel_angular1, -// const Eigen::Matrix& acc_linear1, -// const T dt, -// Eigen::Matrix position2, -// Eigen::Quaternion orientation2, -// Eigen::Matrix vel_linear2, -// Eigen::Matrix vel_angular2, -// Eigen::Matrix acc_linear2) -// { -// // Convert quaternion to eigen -// Eigen::Vector rpy( -// fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), -// fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), -// fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) -// ); -// // 3D material point projection model which matches the one used by r_l. -// T sr = ceres::sin(rpy.x()); -// T cr = ceres::cos(rpy.x()); -// T sp = ceres::sin(rpy.y()); -// T cp = ceres::cos(rpy.y()); -// T sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model -// T cy = ceres::cos(rpy.z()); -// T cpi = 1.0 / cp; -// T tp = ceres::tan(rpy.y()); - -// Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; -// Eigen::Matrix state_tf; - -// tf_pos(0, 0) = (cy * cp); -// tf_pos(0, 1) = (cy * sp * sr - sy * cr); -// tf_pos(0, 2) = (cy * sp * cr + sy * sr); -// tf_pos(1, 0) = (sy * cp); -// tf_pos(1, 1) = (sy * sp * sr + cy * cr); -// tf_pos(1, 2) = (sy * sp * cr - cy * sr); -// tf_pos(2, 0) = (-sp); -// tf_pos(2, 1) = (cp * sr); -// tf_pos(2, 2) = (cp * cr); - -// tf_rot(0, 0) = 1; -// tf_rot(0, 1) = sr * sp * cpi; -// tf_rot(0, 2) = cr * sp * cpi; -// tf_rot(1, 0) = T(0); -// tf_rot(1, 1) = cr; -// tf_rot(1, 2) = -sr; -// tf_rot(2, 0) = T(0); -// tf_rot(2, 1) = sr * cpi; -// tf_rot(2, 2) = cr * cpi; - -// // Project the state -// position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; -// rpy = rpy + tf_rot * vel_angular1 * dt; -// vel_linear2 = vel_linear1 + acc_linear1 * dt; -// vel_angular2 = vel_angular1; -// acc_linear2 = acc_linear1; - -// fuse_core::wrapAngle2D(rpy.x()); -// fuse_core::wrapAngle2D(rpy.y()); -// fuse_core::wrapAngle2D(rpy.z()); - -// // Convert back to quaternion -// orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * -// Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * -// Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); -// } +using Jet = ceres::Jet; /** * @brief Given a state and time delta, predicts a new state - * inner templated version for ceres autodiff @@ -161,11 +66,11 @@ inline void predict_eigen( const Eigen::Matrix& vel_angular1, const Eigen::Matrix& acc_linear1, const double dt, - Eigen::Matrix position2, - Eigen::Quaternion orientation2, - Eigen::Matrix vel_linear2, - Eigen::Matrix vel_angular2, - Eigen::Matrix acc_linear2) + Eigen::Matrix& position2, + Eigen::Quaternion& orientation2, + Eigen::Matrix& vel_linear2, + Eigen::Matrix& vel_angular2, + Eigen::Matrix& acc_linear2) { // Convert quaternion to eigen Eigen::Vector rpy( @@ -247,11 +152,11 @@ inline void predict_eigen( const Eigen::Matrix& vel_angular1, const Eigen::Matrix& acc_linear1, const Jet dt, - Eigen::Matrix position2, - Eigen::Quaternion orientation2, - Eigen::Matrix vel_linear2, - Eigen::Matrix vel_angular2, - Eigen::Matrix acc_linear2) + Eigen::Matrix& position2, + Eigen::Quaternion& orientation2, + Eigen::Matrix& vel_linear2, + Eigen::Matrix& vel_angular2, + Eigen::Matrix& acc_linear2) { // Convert quaternion to eigen Eigen::Vector rpy( @@ -309,13 +214,6 @@ inline void predict_eigen( orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); - - // Project the state - dummy versions - position2 = position1; - orientation2 = orientation1; - vel_linear2 = vel_linear1; - vel_angular2 = vel_angular1; - acc_linear2 = acc_linear1; } /** @@ -348,42 +246,61 @@ inline void predict( T * const acc_linear2) { // conversion from array to eigen - // TODO: check how to map quaternion for rowmajor arrays - const Eigen::Matrix position1_eigen = - Eigen::Map> (position1); - const Eigen::Quaternion orientation1_eigen = - Eigen::Map> (orientation1); - const Eigen::Matrix vel_linear1_eigen = - Eigen::Map> (vel_linear1); - const Eigen::Matrix vel_angular1_eigen = - Eigen::Map> (vel_angular1); - const Eigen::Matrix acc_linear1_eigen = - Eigen::Map> (acc_linear1); + // TODO: Check how to map Eigen::Quaternion to quaternion as a C array: + // Eigen stores component in order x, y, z, w, while in fuse orientation3d + // variable the order is w, x, y, z + Eigen::Map> position1_map(position1); + // Eigen::Map> orientation1_map(orientation1); + Eigen::Quaternion orientation1_map; + orientation1_map.w() = orientation1[0]; + orientation1_map.x() = orientation1[1]; + orientation1_map.y() = orientation1[2]; + orientation1_map.z() = orientation1[3]; + Eigen::Map> vel_linear1_map(vel_linear1); + Eigen::Map> vel_angular1_map(vel_angular1); + Eigen::Map> acc_linear1_map(acc_linear1); + + // TODO: Check how to map directly output variables to eigen + // Using maps fails during compilation probably because Eigen::Map is implicitly converted + // to Eigen::Matrix when is passed to predict_eigen + Eigen::Matrix position2_eigen; + Eigen::Quaternion orientation2_eigen; + Eigen::Matrix vel_linear2_eigen; + Eigen::Matrix vel_angular2_eigen; + Eigen::Matrix acc_linear2_eigen; - Eigen::Matrix position2_eigen = - Eigen::Map> (position2); - Eigen::Quaternion orientation2_eigen = - Eigen::Map> (orientation2); - Eigen::Matrix vel_linear2_eigen = - Eigen::Map> (vel_linear2); - Eigen::Matrix vel_angular2_eigen = - Eigen::Map> (vel_angular2); - Eigen::Matrix acc_linear2_eigen = - Eigen::Map> (acc_linear2); - predict_eigen( - position1_eigen, - orientation1_eigen, - vel_linear1_eigen, - vel_angular1_eigen, - acc_linear1_eigen, + position1_map, + orientation1_map, + vel_linear1_map, + vel_angular1_map, + acc_linear1_map, dt, position2_eigen, orientation2_eigen, vel_linear2_eigen, vel_angular2_eigen, - acc_linear2_eigen); -} + acc_linear2_eigen + ); + + // Convert back to C array + position2[0] = position2_eigen.x(); + position2[1] = position2_eigen.y(); + position2[2] = position2_eigen.z(); + orientation2[0] = orientation2_eigen.w(); + orientation2[1] = orientation2_eigen.x(); + orientation2[2] = orientation2_eigen.y(); + orientation2[3] = orientation2_eigen.z(); + vel_linear2[0] = vel_linear2_eigen.x(); + vel_linear2[1] = vel_linear2_eigen.y(); + vel_linear2[2] = vel_linear2_eigen.z(); + vel_angular2[0] = vel_angular2_eigen.x(); + vel_angular2[1] = vel_angular2_eigen.y(); + vel_angular2[2] = vel_angular2_eigen.z(); + acc_linear2[0] = acc_linear2_eigen.x(); + acc_linear2[1] = acc_linear2_eigen.y(); + acc_linear2[2] = acc_linear2_eigen.z(); + } /** * @brief Given a state and time delta, predicts a new state + computes the Jacobians - * version for predicting new states inside the plugin @@ -413,8 +330,6 @@ inline void predict( fuse_core::Vector3d & vel_angular2, fuse_core::Vector3d & acc_linear2) { - // This is all double code - keep it like this if we want to use also analytic diff, otherwise we - // can call the templated version above from here // Convert quaternion to eigen fuse_core::Vector3d rpy( fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), @@ -467,137 +382,10 @@ inline void predict( fuse_core::wrapAngle2D(rpy.y()); fuse_core::wrapAngle2D(rpy.z()); - // Convert back to quaternion + // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - - // // Compute the transfer function matrix - // state_tf.setZero(); - // // position - // state_tf.block<3, 3>(0, 0) = fuse_core::Matrix3d::Identity(); - // state_tf.block<3, 3>(0, 6) = tf_pos; - // state_tf.block<3, 3>(0, 12) = 0.5 * tf_pos * dt; - // // orientation - // state_tf.block<3, 3>(3, 3) = fuse_core::Matrix3d::Identity(); - // state_tf.block<3, 3>(3, 9) = tf_rot; - // // linear velocity - // state_tf.block<3, 3>(6, 6) = fuse_core::Matrix3d::Identity(); - // state_tf.block<3, 3>(6, 12) = dt * fuse_core::Matrix3d::Identity(); - // // angular velocity - // state_tf.block<3, 3>(9, 9) = fuse_core::Matrix3d::Identity(); - // // linear acceleration - // state_tf.block<3, 3>(12, 12) = fuse_core::Matrix3d::Identity(); - - // if (state_tf_jacobian) - // { - // // TODO: compute the jacobian of the motion model transfer function - // double x_coeff = 0.0; - // double y_coeff = 0.0; - // double z_coeff = 0.0; - // double one_half_at_squared = 0.5 * dt * dt; - - // y_coeff = cy * sp * cr + sy * sr; - // z_coeff = -cy * sp * sr + sy * cr; - // double dFx_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - // double dFR_dR = 1.0 + (cr * tp * vel_angular.y() - sr * tp * vel_angular.z()) * dt; - - // x_coeff = -cy * sp; - // y_coeff = cy * cp * sr; - // z_coeff = cy * cp * cr; - // double dFx_dP = - // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - // one_half_at_squared; - // double dFR_dP = - // (cpi * cpi * sr * vel_angular.y() + cpi * cpi * cr * vel_angular.z()) * dt; - - // x_coeff = -sy * cp; - // y_coeff = -sy * sp * sr - cy * cr; - // z_coeff = -sy * sp * cr + cy * sr; - // double dFx_dY = - // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - // one_half_at_squared; - - // y_coeff = sy * sp * cr - cy * sr; - // z_coeff = -sy * sp * sr - cy * cr; - // double dFy_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - // double dFP_dR = (-sr * vel_angular.y() - cr * vel_angular.z()) * dt; - - // x_coeff = -sy * sp; - // y_coeff = sy * cp * sr; - // z_coeff = sy * cp * cr; - // double dFy_dP = - // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - // one_half_at_squared; - - // x_coeff = cy * cp; - // y_coeff = cy * sp * sr - sy * cr; - // z_coeff = cy * sp * cr + sy * sr; - // double dFy_dY = - // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - // one_half_at_squared; - - // y_coeff = cp * cr; - // z_coeff = -cp * sr; - // double dFz_dR = (y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * one_half_at_squared; - // double dFY_dR = (cr * cpi * vel_angular.y() - sr * cpi * vel_angular.z()) * dt; - - // x_coeff = -cp; - // y_coeff = -sp * sr; - // z_coeff = -sp * cr; - // double dFz_dP = - // (x_coeff * vel_linear.x() + y_coeff * vel_linear.y() + z_coeff * vel_linear.z()) * dt + - // (x_coeff * acc_linear.x() + y_coeff * acc_linear.y() + z_coeff * acc_linear.z()) * - // one_half_at_squared; - // double dFY_dP = - // (sr * tp * cpi * vel_angular.y() + cr * tp * cpi * vel_angular.z()) * dt; - - // *state_tf_jacobian = state_tf; - // (*state_tf_jacobian)(0, 3) = dFx_dR; - // (*state_tf_jacobian)(0, 4) = dFx_dP; - // (*state_tf_jacobian)(0, 5) = dFx_dY; - // (*state_tf_jacobian)(1, 3) = dFy_dR; - // (*state_tf_jacobian)(1, 4) = dFy_dP; - // (*state_tf_jacobian)(1, 5) = dFy_dY; - // (*state_tf_jacobian)(2, 3) = dFz_dR; - // (*state_tf_jacobian)(2, 4) = dFz_dP; - // (*state_tf_jacobian)(3, 3) = dFR_dR; - // (*state_tf_jacobian)(3, 4) = dFR_dP; - // (*state_tf_jacobian)(4, 3) = dFP_dR; - // (*state_tf_jacobian)(5, 3) = dFY_dR; - // (*state_tf_jacobian)(5, 4) = dFY_dP; - // } - - // // Predict the new state - // fuse_core::Vector15d state; - // state.head(3) = position; - // state.segment(3,3) = fuse_core::Vector3d(roll, pitch, yaw); - // state.segment(6,3) = vel_linear; - // state.segment(9,3) = vel_angular; - // state.tail(3) = acc_linear; - - // state = state_tf * state; - - // // Convert back - // position_out = state.head(3); - // fuse_core::wrapAngle2D(state(5)); - // fuse_core::wrapAngle2D(state(4)); - // fuse_core::wrapAngle2D(state(3)); - // Eigen::Quaterniond q = - // Eigen::AngleAxisd(state(5), Eigen::Vector3d::UnitZ()) * - // Eigen::AngleAxisd(state(4), Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(state(3), Eigen::Vector3d::UnitX()); - // orientation_out = q; - // vel_linear_out = state.segment(6,3); - // vel_angular_out = state.segment(9,3); - // acc_linear_out = state.tail(3); + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); } } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp deleted file mode 100644 index 22b71a0bf..000000000 --- a/fuse_models/include/fuse_models/unicycle_3d_predict_no_vec.hpp +++ /dev/null @@ -1,311 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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_MODELS__UNICYCLE_3D_PREDICT_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ - -#include -#include -#include - -#include -#include -#include "covariance_geometry/pose_representation.hpp" - -namespace fuse_models -{ - using Jet = ceres::Jet; - -template -inline void predict( - const T position1_x, - const T position1_y, - const T position1_z, - const T quat1_w, - const T quat1_x, - const T quat1_y, - const T quat1_z, - const T vel_linear1_x, - const T vel_linear1_y, - const T vel_linear1_z, - const T vel_roll1, - const T vel_pitch1, - const T vel_yaw1, - const T acc_linear1_x, - const T acc_linear1_y, - const T acc_linear1_z, - const T dt, - T & position2_x, - T & position2_y, - T & position2_z, - T & quat2_w, - T & quat2_x, - T & quat2_y, - T & quat2_z, - T & vel_linear2_x, - T & vel_linear2_y, - T & vel_linear2_z, - T & vel_roll2, - T & vel_pitch2, - T & vel_yaw2, - T & acc_linear2_x, - T & acc_linear2_y, - T & acc_linear2_z) -{ - // Convert quaternion to eigen - T roll1 = fuse_core::getRoll(quat1_w, quat1_x, quat1_y, quat1_z); - T pitch1 = fuse_core::getPitch(quat1_w, quat1_x, quat1_y, quat1_z); - T yaw1 = fuse_core::getYaw(quat1_w, quat1_x, quat1_y, quat1_z); - fuse_core::wrapAngle2D(roll1); - fuse_core::wrapAngle2D(pitch1); - fuse_core::wrapAngle2D(yaw1); - // 3D material point projection model which matches the one used by r_l. - T sr = ceres::sin(roll1); - T cr = ceres::cos(roll1); - T sp = ceres::sin(pitch1); - T cp = ceres::cos(pitch1); - T sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - T cy = ceres::cos(yaw1); - T cpi = 1.0 / cp; - - // Project the state - position2_x = position1_x + - ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + - 0.5 * ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt * dt; - - position2_y = position1_y + - ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + - 0.5 * ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt * dt; - - position2_z = position1_z + - ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + - 0.5 * ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt * dt; - - T roll2 = roll1 + - (vel_roll1 + sr * sp * cpi * vel_pitch1 + cr * sp * cpi * vel_yaw1) * dt; - T pitch2 = pitch1 + - (T(0) * vel_roll1 + cr * vel_pitch1 - sr * vel_yaw1) * dt; - T yaw2 = yaw1 + - (T(0) * vel_roll1 + sr * cpi * vel_pitch1 + cr * cpi * vel_yaw1) * dt; - - fuse_core::wrapAngle2D(roll2); - fuse_core::wrapAngle2D(pitch2); - fuse_core::wrapAngle2D(yaw2); - - Eigen::Quaternion q; - q.w() = cos(roll2 / T(2)) * cos(pitch2 / T(2)) * cos(yaw2 / T(2)) + - sin(roll2 / T(2)) * sin(pitch2 / T(2)) * sin(yaw2 / T(2)); - q.x() = sin(roll2 / T(2)) * cos(pitch2 / T(2)) * cos(yaw2 / T(2)) - - cos(roll2 / T(2)) * sin(pitch2 / T(2)) * sin(yaw2 / T(2)); - q.y() = cos(roll2 / T(2)) * sin(pitch2 / T(2)) * cos(yaw2 / T(2)) + - sin(roll2 / T(2)) * cos(pitch2 / T(2)) * sin(yaw2 / T(2)); - q.z() = cos(roll2 / T(2)) * cos(pitch2 / T(2)) * sin(yaw2 / T(2)) - - sin(roll2 / T(2)) * sin(pitch2 / T(2)) * cos(yaw2 / T(2)); - q.normalize(); - - quat2_w = q.w(); - quat2_x = q.x(); - quat2_y = q.y(); - quat2_z = q.z(); - - vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; - vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; - vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; - - vel_roll2 = vel_roll1; - vel_pitch2 = vel_pitch1; - vel_yaw2 = vel_yaw1; - - acc_linear2_x = acc_linear1_x; - acc_linear2_y = acc_linear1_y; - acc_linear2_z = acc_linear1_z; -} - -/** - * @brief Given a state and time delta, predicts a new state - * @param[in] position1 - First position (array with x at index 0, y at index 1) - * @param[in] yaw1 - First orientation - * @param[in] vel_linear1 - First velocity (array with x at index 0, y at index 1) - * @param[in] vel_yaw1 - First yaw velocity - * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1) - * @param[in] dt - The time delta across which to predict the state - * @param[out] position2 - Second position (array with x at index 0, y at index 1) - * @param[out] yaw2 - Second orientation - * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1) - * @param[out] vel_yaw2 - Second yaw velocity - * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) - */ -template -inline void predict( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T dt, - T * const position2, - T * const orientation2, - T * const vel_linear2, - T * const vel_angular2, - T * const acc_linear2) -{ - predict( - position1[0], - position1[1], - position1[2], - orientation1[0], - orientation1[1], - orientation1[2], - orientation1[3], - vel_linear1[0], - vel_linear1[1], - vel_linear1[2], - vel_angular1[0], - vel_angular1[1], - vel_angular1[2], - acc_linear1[0], - acc_linear1[1], - acc_linear1[2], - dt, - position2[0], - position2[1], - position2[2], - orientation2[0], - orientation2[1], - orientation2[2], - orientation2[3], - vel_linear2[0], - vel_linear2[1], - vel_linear2[2], - vel_angular2[0], - vel_angular2[1], - vel_angular2[2], - acc_linear2[0], - acc_linear2[1], - acc_linear2[2]); -} - -/** - * @brief Given a state and time delta, predicts a new state + computes the Jacobians - - * version for predicting new states inside the plugin - * @param[in] position - First position - * @param[in] orientation - First orientation - * @param[in] vel_linear - First linear velocity - * @param[in] vel_angular - First angular velocity - * @param[in] acc_linear - First linear acceleration - * @param[in] dt - The time delta across which to predict the state - * @param[out] position_out - Second position - * @param[out] orientation_out - Second orientation - * @param[out] vel_linear_out - Second linear velocity - * @param[out] vel_angular_out - Second angular velocity - * @param[out] acc_linear_out - Second linear acceleration - * @param[out] state_tf_jacobian - Jacobian of the motion model - */ -inline void predict( - const fuse_core::Vector3d & position1, - const fuse_core::Quaternion & orientation1, - const fuse_core::Vector3d & vel_linear1, - const fuse_core::Vector3d & vel_angular1, - const fuse_core::Vector3d & acc_linear1, - const double dt, - fuse_core::Vector3d & position2, - fuse_core::Quaternion & orientation2, - fuse_core::Vector3d & vel_linear2, - fuse_core::Vector3d & vel_angular2, - fuse_core::Vector3d & acc_linear2) -{ - // This is all double code - keep it like this if we want to use also analytic diff, otherwise we - // can call the templated version above from here - // Convert quaternion to eigen - fuse_core::Vector3d rpy( - fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) - ); - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); - // 3D material point projection model which matches the one used by r_l. - double sr = ceres::sin(rpy.x()); - double cr = ceres::cos(rpy.x()); - double sp = ceres::sin(rpy.y()); - double cp = ceres::cos(rpy.y()); - double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - double cy = ceres::cos(rpy.z()); - double cpi = 1.0 / cp; - - fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; - fuse_core::Matrix15d state_tf; - - tf_pos(0, 0) = (cy * cp); - tf_pos(0, 1) = (cy * sp * sr - sy * cr); - tf_pos(0, 2) = (cy * sp * cr + sy * sr); - tf_pos(1, 0) = (sy * cp); - tf_pos(1, 1) = (sy * sp * sr + cy * cr); - tf_pos(1, 2) = (sy * sp * cr - cy * sr); - tf_pos(2, 0) = (-sp); - tf_pos(2, 1) = (cp * sr); - tf_pos(2, 2) = (cp * cr); - - tf_rot(0, 0) = 1.0; - tf_rot(0, 1) = sr * sp * cpi; - tf_rot(0, 2) = cr * sp * cpi; - tf_rot(1, 0) = 0.0; - tf_rot(1, 1) = cr; - tf_rot(1, 2) = -sr; - tf_rot(2, 0) = 0.0; - tf_rot(2, 1) = sr * cpi; - tf_rot(2, 2) = cr * cpi; - - // Project the state - position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; - rpy = rpy + tf_rot * vel_angular1 * dt; - vel_linear2 = vel_linear1 + acc_linear1 * dt; - vel_angular2 = vel_angular1; - acc_linear2 = acc_linear1; - - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); - - // Convert back to quaternion - orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - - std::cout << "Predicted position: " << position2.transpose() << std::endl; - std::cout << "Predicted orientation: w: " << orientation2.w() << " x:" << orientation2.x() << " y:" << orientation2.y() << " z:" << orientation2.z() << std::endl; -} -} // namespace fuse_models - -#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index 056dfadbc..9d219e8f8 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -148,7 +148,7 @@ bool Unicycle3DStateCostFunctor::operator()( T * residual) const { T position_pred[3] {T(0.0)}; - T orientation_pred[4] {T(0.0)}; + T orientation_pred[4] {T(0.0)}; // storage order: w, x, y, z T vel_linear_pred[3] {T(0.0)}; T vel_angular_pred[3] {T(0.0)}; T acc_linear_pred[3] {T(0.0)}; @@ -166,9 +166,7 @@ bool Unicycle3DStateCostFunctor::operator()( acc_linear_pred); Eigen::Map> residuals_map(residual); - // Eigen::Map> q_pred(orientation_pred), q2(orientation2); - // Eigen::Quaternion q_res = q2.inverse() * q_pred; - + T orientation_pred_rpy[3]; T orientation2_rpy[3]; @@ -189,10 +187,6 @@ bool Unicycle3DStateCostFunctor::operator()( residuals_map(0) = T(position2[0] - position_pred[0]); residuals_map(1) = T(position2[1] - position_pred[1]); residuals_map(2) = T(position2[2] - position_pred[2]); - // residuals_map(3) = T(q_res.x()); - // residuals_map(4) = T(q_res.y()); - // residuals_map(5) = T(q_res.z()); - // residuals_map(6) = T(q_res.w()); residuals_map(3) = T(orientation2_rpy[0] - orientation_pred_rpy[0]); residuals_map(4) = T(orientation2_rpy[1] - orientation_pred_rpy[1]); residuals_map(5) = T(orientation2_rpy[2] - orientation_pred_rpy[2]); @@ -206,6 +200,10 @@ bool Unicycle3DStateCostFunctor::operator()( residuals_map(13) = T(acc_linear2[1] - acc_linear_pred[1]); residuals_map(14) = T(acc_linear2[2] - acc_linear_pred[2]); + fuse_core::wrapAngle2D(residuals_map(3)); + fuse_core::wrapAngle2D(residuals_map(4)); + fuse_core::wrapAngle2D(residuals_map(5)); + // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. residuals_map.applyOnTheLeft(A_.template cast()); diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index e4825b941..c74b2b353 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,7 +5,8 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition - # test_unicycle_3d_predict + test_unicycle_3d_predict + test_unicycle_3d_state_cost_function ) foreach(test_name ${TEST_TARGETS}) diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index 59da3e516..009af8d0f 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -39,7 +39,7 @@ #include #include -// #include +#include TEST(Predict, predictDirectVals) { @@ -152,63 +152,510 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // TODO: continue with out of plane motion } -TEST(Predict, predictJacobians) +TEST(Predict, predictFromDoublePointers) { - // atm this test is doig nothing, we don't have a way to compute the jacobian analytically - fuse_core::Vector3d position1 (0.0, 0.0, 0.0); - fuse_core::Quaternion orientation1 (1.0, 0.0, 0.0, 0.0); - fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); - fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); - fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); - + const double position1[3] {0.0, 0.0, 0.0}; + const double orientation1[4] {1.0, 0.0, 0.0, 0.0}; + const double vel_linear1[3] {1.0, 0.0, 0.0}; + const double vel_angular1[3] {0.0, 0.0, 1.570796327}; + const double acc_linear1[3] {1.0, 0.0, 0.0}; const double dt = 0.1; - fuse_core::Vector3d position2; - fuse_core::Quaternion orientation2; - fuse_core::Vector3d vel_linear2; - fuse_core::Vector3d vel_angular2; - fuse_core::Vector3d acc_linear2; + double position2[3] {1000.0, 0.0, 0.0}; + double orientation2[4]; + double vel_linear2[3]; + double vel_angular2[3]; + double acc_linear2[3]; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); - // Predict and compute Jacobian using autodiff - using Jet = ceres::Jet; - const Jet jet_dt (dt); - const Jet jet_position1[3] {{position1.x(), 0}, {position1.y(), 1}, {position1.z(), 2}}; - const Jet jet_orientation1[3] {{orientation1.x(), 0}, {orientation1.y(), 1}, {orientation1.z(), 2}}; - const Jet jet_vel_linear1[3] {{vel_linear1.x(), 0}, {vel_linear1.y(), 1}, {vel_linear1.z(), 2}}; - const Jet jet_vel_angular1[3] {{vel_angular1.x(), 0}, {vel_angular1.y(), 1}, {vel_angular1.z(), 2}}; - const Jet jet_acc_linear1[3] {{acc_linear1.x(), 0}, {acc_linear1.y(), 1}, {acc_linear1.z(), 2}}; + fuse_core::Quaternion q = + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - Jet jet_position2[3], jet_orientation2[3], jet_vel_linear2[3], jet_vel_angular2[3], jet_acc_linear2[3]; + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); + EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); + EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); + EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // // Carry on with the output state from last time - show in-place update support + // fuse_models::predict( + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); + + // q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + // EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + // EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); + // EXPECT_DOUBLE_EQ(0.0, position2[2]); + // EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); + // EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); + // EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); + // EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); + // EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + // EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + // EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // // // Use non-zero Y values + // vel_linear1[1] = -1.0; + // vel_angular1[2] = -1.570796327; + // acc_linear1[1] = -1.0; + + // fuse_models::predict( + // position1, + // orientation1, + // vel_linear1, + // vel_angular1, + // acc_linear1, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); + + // q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + // EXPECT_DOUBLE_EQ(0.105, position2[0]); + // EXPECT_DOUBLE_EQ(-0.105,position2[1]); + // EXPECT_DOUBLE_EQ(0.0, position2[2]); + // EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); + // EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); + // EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); + // EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); + // EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + // EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + // EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + // EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + // EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + // // TODO: continue with out of plane motion +} + +TEST(Predict, predictFromJetPointers) +{ + using Jet = ceres::Jet; + + Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet orientation1[4] = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; + Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + const Jet dt = Jet(0.1); + Jet position2[3]; + Jet orientation2[4]; + Jet vel_linear2[3]; + Jet vel_angular2[3]; + Jet acc_linear2[3]; fuse_models::predict( - jet_position1, - jet_orientation1, - jet_vel_linear1, - jet_vel_angular1, - jet_acc_linear1, - jet_dt, - jet_position2, - jet_orientation2, - jet_vel_linear2, - jet_vel_angular2, - jet_acc_linear2); - - fuse_core::Matrix15d J_autodiff; - J_autodiff << jet_position2[0].v, jet_position2[1].v, jet_position2[2].v, - jet_orientation2[0].v, jet_orientation2[1].v, jet_orientation2[2].v, - jet_vel_linear2[0].v, jet_vel_linear2[1].v, jet_vel_linear2[2].v, - jet_vel_angular2[0].v, jet_vel_angular2[1].v, jet_vel_angular2[2].v, - jet_acc_linear2[0].v, jet_acc_linear2[1].v, jet_acc_linear2[2].v; - J_autodiff.transposeInPlace(); - - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + Eigen::Quaternion q = + Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Use non-zero Y values + vel_linear1[1] = Jet(-1.0); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + // TODO: continue with out of plane motion +} + +TEST(Predict, predictFromEigenMatrixDouble) +{ + Eigen::Matrix position1 = {0.0, 0.0, 0.0}; + Eigen::Quaterniond orientation1 = {1.0, 0.0, 0.0, 0.0}; + Eigen::Matrix vel_linear1 = {1.0, 0.0, 0.0}; + Eigen::Matrix vel_angular1 = {0.0, 0.0, 1.570796327}; + Eigen::Matrix acc_linear1 = {1.0, 0.0, 0.0}; + const double dt = 0.1; + Eigen::Matrix position2; + Eigen::Quaterniond orientation2; + Eigen::Matrix vel_linear2; + Eigen::Matrix vel_angular2; + Eigen::Matrix acc_linear2; + + fuse_models::predict_eigen( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + fuse_core::Quaternion q = + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict_eigen( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - std::cout << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt); - // EXPECT_MATRIX_NEAR(J_autodiff, J_analytic, std::numeric_limits::epsilon()) - // << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt); - // << "\nAnalytic Jacobian =\n" << J_analytic.format(HeavyFmt); + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Use non-zero Y values + vel_linear1[1] = -1.0; + vel_angular1[2] = -1.570796327; + acc_linear1[1] = -1.0; + + fuse_models::predict_eigen( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(-0.105,position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + // TODO: continue with out of plane motion } + +TEST(Predict, predictFromEigenMatrixJet) +{ + using Jet = ceres::Jet; + Eigen::Matrix position1 = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Eigen::Quaternion orientation1 = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; + Eigen::Matrix vel_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Eigen::Matrix vel_angular1 = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; + Eigen::Matrix acc_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; + const Jet dt = Jet(0.1); + Eigen::Matrix position2; + Eigen::Quaternion orientation2; + Eigen::Matrix vel_linear2; + Eigen::Matrix vel_angular2; + Eigen::Matrix acc_linear2; + + fuse_models::predict_eigen( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + Eigen::Quaternion q = + Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict_eigen( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Use non-zero Y values + vel_linear1[1] = Jet(-1.0); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict_eigen( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); + EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); + EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); + EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + // TODO: continue with out of plane motion +} \ No newline at end of file diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp index 60a98244b..c9744c768 100644 --- a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -41,7 +41,8 @@ #include #include -#include +// #include +#include TEST(CostFunction, evaluateCostFunction) { @@ -54,7 +55,7 @@ TEST(CostFunction, evaluateCostFunction) const double dt{0.1}; const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; - const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; + // const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; // Evaluate cost function const double position1[] = {0.0, 0.0, 0.0}; @@ -64,37 +65,37 @@ TEST(CostFunction, evaluateCostFunction) const double acc_linear1[] = {1.0, 0.0}; const double position2[] = {0.105, 0.0}; - const double yaw2[] = {0.1570796327}; + const double orientation2[] = {0.1570796327}; const double vel_linear2[] = {1.1, 0.0}; const double vel_yaw2[] = {1.570796327}; const double acc_linear2[] = {1.0, 0.0}; const double * parameters[] = { - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 + position1, orientation1, vel_linear1, vel_yaw1, acc_linear1, + position2, orientation2, vel_linear2, vel_yaw2, acc_linear2 }; - // fuse_core::Vector8d residuals; + fuse_core::Vector8d residuals; - // const auto & block_sizes = cost_function.parameter_block_sizes(); - // const auto num_parameter_blocks = block_sizes.size(); +// const auto & block_sizes = cost_function.parameter_block_sizes(); +// const auto num_parameter_blocks = block_sizes.size(); - // const auto num_residuals = cost_function.num_residuals(); +// const auto num_residuals = cost_function.num_residuals(); - // std::vector J(num_parameter_blocks); - // std::vector jacobians(num_parameter_blocks); +// std::vector J(num_parameter_blocks); +// std::vector jacobians(num_parameter_blocks); - // for (size_t i = 0; i < num_parameter_blocks; ++i) { - // J[i].resize(num_residuals, block_sizes[i]); - // jacobians[i] = J[i].data(); - // } +// for (size_t i = 0; i < num_parameter_blocks; ++i) { +// J[i].resize(num_residuals, block_sizes[i]); +// jacobians[i] = J[i].data(); +// } - // EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); +// EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); - // // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 - // // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 - // EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); +// // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 +// // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 +// EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); // // Check jacobians are correct using a gradient checker // ceres::NumericDiffOptions numeric_diff_options; @@ -116,9 +117,9 @@ TEST(CostFunction, evaluateCostFunction) // // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle3DStateCostFunctor(dt, sqrt_information)); auto num_parameter_blocks = cost_function_autodiff.parameter_block_sizes().size(); auto num_residuals = cost_function_autodiff.num_residuals(); // Evaluate cost function that uses automatic differentiation @@ -126,7 +127,7 @@ TEST(CostFunction, evaluateCostFunction) std::vector jacobians_autodiff(num_parameter_blocks); for (size_t i = 0; i < num_parameter_blocks; ++i) { - J_autodiff[i].resize(num_residuals, block_sizes[i]); + J_autodiff[i].resize(num_residuals, cost_function_autodiff.parameter_block_sizes()[i]); jacobians_autodiff[i] = J_autodiff[i].data(); } @@ -135,12 +136,12 @@ TEST(CostFunction, evaluateCostFunction) parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + // const Eigen::IOFormat HeavyFmt( + // Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) { - EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); - } + // for (size_t i = 0; i < num_parameter_blocks; ++i) { + // EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) + // << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) + // << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + // } } From 3fb62bdc1a7f6d9b5ea4c84ad2b2f9c095e22224 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:13:14 +0100 Subject: [PATCH 021/116] Update README.md Co-authored-by: Patrick Roncagliolo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7f4fd5d89..3908121b1 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # fuse -**NOTE**: The `humble` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done! +**NOTE**: The `rolling` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done! The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications include state estimation, localization, mapping, and calibration. From 10c006547220fadfdc3f3567dae9ee67bf9e6cca Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:15:44 +0100 Subject: [PATCH 022/116] Update typo in fuse_constraints/fuse_plugins.xml Co-authored-by: Patrick Roncagliolo --- fuse_constraints/fuse_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index 6dee9a605..1c99a028a 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -8,7 +8,7 @@ A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of - the 2D angular acceleration. + the 3D angular acceleration. From df856ca2d16432ebfe5f49730bb7f4571f7a8eb7 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:16:08 +0100 Subject: [PATCH 023/116] Update typo in fuse_constraints/fuse_plugins.xml Co-authored-by: Patrick Roncagliolo --- fuse_constraints/fuse_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index 1c99a028a..d4ec86a9f 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -20,7 +20,7 @@ A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of - the 2D linear acceleration. + the 3D linear acceleration. From 92a3a69310ab8eb37f1476da5f7cb5f02c6a6a22 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:16:22 +0100 Subject: [PATCH 024/116] Update typo in fuse_constraints/fuse_plugins.xml Co-authored-by: Patrick Roncagliolo --- fuse_constraints/fuse_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index d4ec86a9f..1cd67406c 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -50,7 +50,7 @@ A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of - the 2D angular velocity. + the 3D angular velocity. From 3fac148b898a2f38fc80415b11fa12502e9e04bd Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:16:39 +0100 Subject: [PATCH 025/116] Update typo in fuse_constraints/fuse_plugins.xml Co-authored-by: Patrick Roncagliolo --- fuse_constraints/fuse_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index 1cd67406c..c69608a7c 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -62,7 +62,7 @@ A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of - the 2D linear velocity. + the 3D linear velocity. From 211e0ebf2b49899d44744bce4588321b8f1dfb3f Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:19:56 +0100 Subject: [PATCH 026/116] Update fuse_constraints/fuse_plugins.xml Co-authored-by: Patrick Roncagliolo --- fuse_constraints/fuse_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index c69608a7c..7f23443ce 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -84,7 +84,7 @@ - A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose in quat. + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose. From 89656fb237bf51ffbfe5621f4c06bad83817607e Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:45:44 +0100 Subject: [PATCH 027/116] Update copyright fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp Co-authored-by: Patrick Roncagliolo --- .../include/fuse_constraints/normal_prior_pose_3d.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 364df3e90..4318c18b0 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2020, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without From 486336b87fb8ef025d5af8eaa91b15ac6e3c7d84 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:55:28 +0100 Subject: [PATCH 028/116] Fix: removed double new line Co-authored-by: Patrick Roncagliolo --- fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index 983119a49..1b02e322d 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -116,7 +116,7 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const << " position variable: " << variables().at(0) << "\n" << " orientation variable: " << variables().at(1) << "\n" << " mean: " << mean().transpose() << "\n" - << " sqrt_info: \n" << sqrtInformation() << "\n"; + << " sqrt_info: " << sqrtInformation() << "\n"; if (loss()) { stream << " loss: "; From a09db78aa731eafdef996b3cf486fabe6c95cb79 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:55:51 +0100 Subject: [PATCH 029/116] Update copyright in fuse_constraints/src/normal_prior_pose_3d.cpp Co-authored-by: Patrick Roncagliolo --- fuse_constraints/src/normal_prior_pose_3d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index bc0653370..f5ef3987a 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without From f6e78fbcfe33d8c15564fd6f187baea2019a077b Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Mon, 20 Nov 2023 16:00:25 +0000 Subject: [PATCH 030/116] Cleaning is_angular_3d method in sensor config --- .../fuse_models/common/sensor_config.hpp | 29 ------------------- 1 file changed, 29 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index 6967ec6b3..3132cc099 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -194,35 +194,6 @@ std::enable_if_t::value, size_t> toIndex(const std::string return 0u; } -// /** -// * @brief Method that converts from 3D angular axis dimension names to index values -// * -// * This method is enabled only for variable Orientation3DStamped -// * -// * @param[in] dimension - The dimension name to convert -// * @return the index of the enumerated dimension for that type -// * @throws runtime_error if the dimension name is invalid -// */ -// template<> -// std::enable_if_t::value, size_t> toIndex(const std::string & dimension) -// { -// auto lower_dim = boost::algorithm::to_lower_copy(dimension); -// if (lower_dim == "roll" || lower_dim == "x") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 3UL; -// // Trick to obtain indices of roll, pitch, yaw as 3, 4, 5 relative to 3d pose_mean -// } -// if (lower_dim == "pitch" || lower_dim == "y") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 3UL; -// } -// if (lower_dim == "yaw" || lower_dim == "z") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 3UL; -// } - -// throwDimensionError(dimension); - -// return 0u; -// } - /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * From c6a0daf6a23c8e3aa50db1560c1575f7758e060c Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 21 Nov 2023 09:51:44 +0000 Subject: [PATCH 031/116] Revert "Cleaning is_angular_3d method in sensor config" This reverts commit 0b17df462144632f144049f21498a7527b260c55. --- .../fuse_models/common/sensor_config.hpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index 3132cc099..6967ec6b3 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -194,6 +194,35 @@ std::enable_if_t::value, size_t> toIndex(const std::string return 0u; } +// /** +// * @brief Method that converts from 3D angular axis dimension names to index values +// * +// * This method is enabled only for variable Orientation3DStamped +// * +// * @param[in] dimension - The dimension name to convert +// * @return the index of the enumerated dimension for that type +// * @throws runtime_error if the dimension name is invalid +// */ +// template<> +// std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +// { +// auto lower_dim = boost::algorithm::to_lower_copy(dimension); +// if (lower_dim == "roll" || lower_dim == "x") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 3UL; +// // Trick to obtain indices of roll, pitch, yaw as 3, 4, 5 relative to 3d pose_mean +// } +// if (lower_dim == "pitch" || lower_dim == "y") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 3UL; +// } +// if (lower_dim == "yaw" || lower_dim == "z") { +// return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 3UL; +// } + +// throwDimensionError(dimension); + +// return 0u; +// } + /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * From c1b1edef3cfe0abc69d83c97b031cb8efb30232b Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 09:32:01 +0000 Subject: [PATCH 032/116] Fix: delta orientation cost function residuals static map --- .../normal_delta_orientation_3d_cost_functor.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index 5fa881056..988c623bb 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -120,9 +120,7 @@ class NormalDeltaOrientation3DCostFunctor // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - // TODO: Probably here we can set 3 as row dimension instead of Eigen::Dynamic - // Eigen::Map> residuals_map(residuals, A_.rows()); - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals); residuals_map.applyOnTheLeft(A_.template cast()); return true; From ae84092e604f8065f4181847b767bb31b6f501a9 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 11:48:19 +0000 Subject: [PATCH 033/116] Add independent param for pose delta --- .../include/fuse_models/parameters/imu_3d_params.hpp | 10 +++++----- .../fuse_models/parameters/odometry_3d_params.hpp | 12 +++++------- fuse_models/src/imu_3d.cpp | 1 + fuse_models/src/odometry_3d.cpp | 1 + 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 2fc3b2b25..1aca86f61 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -125,12 +125,12 @@ struct Imu3DParams : public ParameterBase fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); if (differential) { + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); // TODO: understand if this is needed - // independent = fuse_core::getParam( - // interfaces, fuse_core::joinParameterName( - // ns, - // "independent"), - // independent); // use_twist_covariance = // fuse_core::getParam( // interfaces, fuse_core::joinParameterName( diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 6d6c6d95f..0ce0aac16 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -144,13 +144,11 @@ struct Odometry3DParams : public ParameterBase pose_target_frame); if (differential) { - // This can be avoided since we are calculating relative pose covariance with - // covariance_geometry - // independent = fuse_core::getParam( - // interfaces, fuse_core::joinParameterName( - // ns, - // "independent"), - // independent); + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); // use_twist_covariance = // fuse_core::getParam( // interfaces, fuse_core::joinParameterName( diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 46f4f5945..f20f5d838 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -304,6 +304,7 @@ void Imu3D::processDifferential( device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, params_.pose_loss, {}, diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 493b5a152..3603e2774 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -214,6 +214,7 @@ void Odometry3D::processDifferential( device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, params_.pose_loss, params_.position_indices, From 1072fe4341b4bb5c50e26405a24a1a0c81378437 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 13:59:05 +0000 Subject: [PATCH 034/116] Add test absolute pose 3d stamped constraint --- ...st_absolute_pose_3d_stamped_constraint.cpp | 202 ++++++++++++++++++ 1 file changed, 202 insertions(+) diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index 9aa40bcf1..990a2514d 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -80,6 +80,33 @@ TEST(AbsolutePose3DStampedConstraint, Constructor) mean, cov)); } +TEST(AbsolutePose3DStampedConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector7d mean_partial; + mean_partial << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix5d cov_partial; + /* *INDENT-OFF* */ + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedConstraint constraint( + "test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); +} + TEST(AbsolutePose3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct @@ -122,6 +149,62 @@ TEST(AbsolutePose3DStampedConstraint, Covariance) EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); } +TEST(AbsolutePose3DStampedConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector7d mean; + mean << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, + mean, cov, variable_indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + // TODO: check if this is correct + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + // expected_sqrt_info << 2.12658752275893, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + // 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + // 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + // 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + // 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, + 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, + 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, + 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, + 0. , 0.0, 0. , 0. , 0. , 0.68147774; + + fuse_core::Matrix6d expected_cov; + + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-8); +} + TEST(AbsolutePose3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are @@ -238,6 +321,125 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); } +TEST(AbsolutePose3DStampedConstraint, OptimizationPartial) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables. Version for partial measurements + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = 0.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector7d mean; + mean << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.2, 0.3, 0.4, 0.5, + 0.2, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov, + variable_indices); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); +} + TEST(AbsolutePose3DStampedConstraint, Serialization) { // Construct a constraint From 55280dc4ca07141376d24c45f5963f8452281aaf Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 14:02:30 +0000 Subject: [PATCH 035/116] unicycle 3d predict refactor + test --- .../fuse_models/unicycle_3d_predict.hpp | 398 ++++----- fuse_models/test/test_unicycle_3d_predict.cpp | 843 +++++++++--------- 2 files changed, 568 insertions(+), 673 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index c819fd9e2..de434963d 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -34,199 +34,135 @@ #ifndef FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ #define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ -#include #include -#include #include #include namespace fuse_models { -using Jet = ceres::Jet; /** - * @brief Given a state and time delta, predicts a new state - - * inner templated version for ceres autodiff - * @param[in] position1 - First position - * @param[in] orientation1 - First orientation - * @param[in] vel_linear1 - First linear velocity - * @param[in] vel_angular1 - First angular velocity - * @param[in] acc_linear1 - First linear acceleration + * @brief Given a state and time delta, predicts a new state + * @param[in] position1_x - First X position + * @param[in] position1_y - First Y position + * @param[in] position1_z - First Z position + * @param[in] orientation1_r - First roll orientation + * @param[in] orientation1_p - First pitch orientation + * @param[in] orientation1_y - First yaw orientation + * @param[in] vel_linear1_x - First X velocity + * @param[in] vel_linear1_y - First Y velocity + * @param[in] vel_linear1_z - First Z velocity + * @param[in] vel_angular1_r - First roll velocity + * @param[in] vel_angular1_p - First pitch velocity + * @param[in] vel_angular1_y - First yaw velocity + * @param[in] acc_linear1_x - First X acceleration + * @param[in] acc_linear1_y - First Y acceleration + * @param[in] acc_linear1_z - First Z acceleration * @param[in] dt - The time delta across which to predict the state - * @param[out] position2 - Second position - * @param[out] orientation2 - Second orientation - * @param[out] vel_linear2 - Second linear velocity - * @param[out] vel_angular2 - Second angular velocity - * @param[out] acc_linear2 - Second linear acceleration + * @param[out] position2_x - Second X position + * @param[out] position2_y - Second Y position + * @param[out] position2_z - Second Z position + * @param[out] orientation2_r - Second roll orientation + * @param[out] orientation2_p - Second pitch orientation + * @param[out] orientation2_y - Second yaw orientation + * @param[out] vel_linear2_x - Second X velocity + * @param[out] vel_linear2_y - Second Y velocity + * @param[out] vel_linear2_z - Second Z velocity + * @param[out] vel_angular_r - Second roll velocity + * @param[out] vel_angular_p - Second pitch velocity + * @param[out] vel_angular_y - Second yaw velocity + * @param[out] acc_linear2_x - Second X acceleration + * @param[out] acc_linear2_y - Second Y acceleration + * @param[out] acc_linear2_z - Second Z acceleration */ -inline void predict_eigen( - const Eigen::Matrix& position1, - const Eigen::Quaternion& orientation1, - const Eigen::Matrix& vel_linear1, - const Eigen::Matrix& vel_angular1, - const Eigen::Matrix& acc_linear1, - const double dt, - Eigen::Matrix& position2, - Eigen::Quaternion& orientation2, - Eigen::Matrix& vel_linear2, - Eigen::Matrix& vel_angular2, - Eigen::Matrix& acc_linear2) +template +inline void predict( + const T position1_x, + const T position1_y, + const T position1_z, + const T orientation1_r, + const T orientation1_p, + const T orientation1_y, + const T vel_linear1_x, + const T vel_linear1_y, + const T vel_linear1_z, + const T vel_angular1_r, + const T vel_angular1_p, + const T vel_angular1_y, + const T acc_linear1_x, + const T acc_linear1_y, + const T acc_linear1_z, + const T dt, + T & position2_x, + T & position2_y, + T & position2_z, + T & orientation2_r, + T & orientation2_p, + T & orientation2_y, + T & vel_linear2_x, + T & vel_linear2_y, + T & vel_linear2_z, + T & vel_angular2_r, + T & vel_angular2_p, + T & vel_angular2_y, + T & acc_linear2_x, + T & acc_linear2_y, + T & acc_linear2_z) { - // Convert quaternion to eigen - Eigen::Vector rpy( - fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) - ); - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); // 3D material point projection model which matches the one used by r_l. - double sr = ceres::sin(rpy.x()); - double cr = ceres::cos(rpy.x()); - double sp = ceres::sin(rpy.y()); - double cp = ceres::cos(rpy.y()); - double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - double cy = ceres::cos(rpy.z()); - double cpi = 1.0 / cp; - - Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; - Eigen::Matrix state_tf; - - tf_pos(0, 0) = (cy * cp); // vx1 to x2 - tf_pos(0, 1) = (cy * sp * sr - sy * cr); // vy1 to x2 - tf_pos(0, 2) = (cy * sp * cr + sy * sr); // vz1 to x2 - tf_pos(1, 0) = (sy * cp); // vx1 to y2 - tf_pos(1, 1) = (sy * sp * sr + cy * cr); // vy1 to y2 - tf_pos(1, 2) = (sy * sp * cr - cy * sr); // vz1 to y2 - tf_pos(2, 0) = (-sp); // vx1 to z2 - tf_pos(2, 1) = (cp * sr); // vy1 to z2 - tf_pos(2, 2) = (cp * cr); // vz1 to z2 - - tf_rot(0, 0) = 1; - tf_rot(0, 1) = sr * sp * cpi; - tf_rot(0, 2) = cr * sp * cpi; - tf_rot(1, 0) = double(0); - tf_rot(1, 1) = cr; - tf_rot(1, 2) = -sr; - tf_rot(2, 0) = double(0); - tf_rot(2, 1) = sr * cpi; - tf_rot(2, 2) = cr * cpi; + T sr = ceres::sin(orientation1_r); + T cr = ceres::cos(orientation1_r); + T sp = ceres::sin(orientation1_p); + T cp = ceres::cos(orientation1_p); + T sy = ceres::sin(orientation1_y); + T cy = ceres::cos(orientation1_y); + T cpi = T(1.0) / cp; // Project the state - position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; - rpy = rpy + tf_rot * vel_angular1 * dt; - vel_linear2 = vel_linear1 + acc_linear1 * dt; - vel_angular2 = vel_angular1; - acc_linear2 = acc_linear1; - - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); - - // Convert back to quaternion - orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * - Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * - Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); -} - -/** - * @brief Given a state and time delta, predicts a new state - - * inner templated version for ceres autodiff - * @param[in] position1 - First position - * @param[in] orientation1 - First orientation - * @param[in] vel_linear1 - First linear velocity - * @param[in] vel_angular1 - First angular velocity - * @param[in] acc_linear1 - First linear acceleration - * @param[in] dt - The time delta across which to predict the state - * @param[out] position2 - Second position - * @param[out] orientation2 - Second orientation - * @param[out] vel_linear2 - Second linear velocity - * @param[out] vel_angular2 - Second angular velocity - * @param[out] acc_linear2 - Second linear acceleration - */ -inline void predict_eigen( - const Eigen::Matrix& position1, - const Eigen::Quaternion& orientation1, - const Eigen::Matrix& vel_linear1, - const Eigen::Matrix& vel_angular1, - const Eigen::Matrix& acc_linear1, - const Jet dt, - Eigen::Matrix& position2, - Eigen::Quaternion& orientation2, - Eigen::Matrix& vel_linear2, - Eigen::Matrix& vel_angular2, - Eigen::Matrix& acc_linear2) -{ - // Convert quaternion to eigen - Eigen::Vector rpy( - fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) - ); - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); - // 3D material point projection model which matches the one used by r_l. - Jet sr = ceres::sin(rpy.x()); - Jet cr = ceres::cos(rpy.x()); - Jet sp = ceres::sin(rpy.y()); - Jet cp = ceres::cos(rpy.y()); - Jet sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - Jet cy = ceres::cos(rpy.z()); - Jet cpi = 1.0 / cp; - - Eigen::Matrix tf_pos, tf_rot, tf_vel, tf_acc; - Eigen::Matrix state_tf; - - tf_pos(0, 0) = (cy * cp); - tf_pos(0, 1) = (cy * sp * sr - sy * cr); - tf_pos(0, 2) = (cy * sp * cr + sy * sr); - tf_pos(1, 0) = (sy * cp); - tf_pos(1, 1) = (sy * sp * sr + cy * cr); - tf_pos(1, 2) = (sy * sp * cr - cy * sr); - tf_pos(2, 0) = (-sp); - tf_pos(2, 1) = (cp * sr); - tf_pos(2, 2) = (cp * cr); - - tf_rot(0, 0) = Jet(1); - tf_rot(0, 1) = sr * sp * cpi; - tf_rot(0, 2) = cr * sp * cpi; - tf_rot(1, 0) = Jet(0); - tf_rot(1, 1) = cr; - tf_rot(1, 2) = -sr; - tf_rot(2, 0) = Jet(0); - tf_rot(2, 1) = sr * cpi; - tf_rot(2, 2) = cr * cpi; + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * T(0.5) * dt * dt; + position2_y = position1_y + + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * T(0.5) * dt * dt; + position2_z = position1_z + + ((-sp) * vel_linear1_x + (cp * sr) + vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (cp * sr) + acc_linear1_y + (cp * cr) * acc_linear1_z) * T(0.5) * dt * dt; + + orientation2_r = orientation1_r + + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; - // Project the state - position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; - rpy = rpy + tf_rot * vel_angular1 * dt; - vel_linear2 = vel_linear1 + acc_linear1 * dt; - vel_angular2 = vel_angular1; - acc_linear2 = acc_linear1; + vel_angular2_r = vel_angular1_r; + vel_angular2_p = vel_angular1_p; + vel_angular2_y = vel_angular1_y; - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; - // Convert back to quaternion - orientation2 = Eigen::AngleAxis(rpy.z(), Eigen::Matrix::UnitZ()) * - Eigen::AngleAxis(rpy.y(), Eigen::Matrix::UnitY()) * - Eigen::AngleAxis(rpy.x(), Eigen::Matrix::UnitX()); -} + fuse_core::wrapAngle2D(orientation2_r); + fuse_core::wrapAngle2D(orientation2_p); + fuse_core::wrapAngle2D(orientation2_y); + } /** * @brief Given a state and time delta, predicts a new state - - * Templated version to be called from ceres autodiff costfunction * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) - * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] orientation1 - First orientation (array with roll at index 0, pitch at index 1, yaw at index 2) * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) * @param[in] dt - The time delta across which to predict the state - * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) - * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[out] orientation2 - Second orientation (array with roll at index 0, pitch at index 1, yaw at index 2) * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) @@ -245,77 +181,52 @@ inline void predict( T * const vel_angular2, T * const acc_linear2) { - // conversion from array to eigen - // TODO: Check how to map Eigen::Quaternion to quaternion as a C array: - // Eigen stores component in order x, y, z, w, while in fuse orientation3d - // variable the order is w, x, y, z - Eigen::Map> position1_map(position1); - // Eigen::Map> orientation1_map(orientation1); - Eigen::Quaternion orientation1_map; - orientation1_map.w() = orientation1[0]; - orientation1_map.x() = orientation1[1]; - orientation1_map.y() = orientation1[2]; - orientation1_map.z() = orientation1[3]; - Eigen::Map> vel_linear1_map(vel_linear1); - Eigen::Map> vel_angular1_map(vel_angular1); - Eigen::Map> acc_linear1_map(acc_linear1); - - // TODO: Check how to map directly output variables to eigen - // Using maps fails during compilation probably because Eigen::Map is implicitly converted - // to Eigen::Matrix when is passed to predict_eigen - Eigen::Matrix position2_eigen; - Eigen::Quaternion orientation2_eigen; - Eigen::Matrix vel_linear2_eigen; - Eigen::Matrix vel_angular2_eigen; - Eigen::Matrix acc_linear2_eigen; - - predict_eigen( - position1_map, - orientation1_map, - vel_linear1_map, - vel_angular1_map, - acc_linear1_map, + predict( + position1[0], + position1[1], + position1[2], + orientation1[0], + orientation1[1], + orientation1[2], + vel_linear1[0], + vel_linear1[1], + vel_linear1[2], + vel_angular1[0], + vel_angular1[1], + vel_angular1[2], + acc_linear1[0], + acc_linear1[1], + acc_linear1[2], dt, - position2_eigen, - orientation2_eigen, - vel_linear2_eigen, - vel_angular2_eigen, - acc_linear2_eigen - ); - - // Convert back to C array - position2[0] = position2_eigen.x(); - position2[1] = position2_eigen.y(); - position2[2] = position2_eigen.z(); - orientation2[0] = orientation2_eigen.w(); - orientation2[1] = orientation2_eigen.x(); - orientation2[2] = orientation2_eigen.y(); - orientation2[3] = orientation2_eigen.z(); - vel_linear2[0] = vel_linear2_eigen.x(); - vel_linear2[1] = vel_linear2_eigen.y(); - vel_linear2[2] = vel_linear2_eigen.z(); - vel_angular2[0] = vel_angular2_eigen.x(); - vel_angular2[1] = vel_angular2_eigen.y(); - vel_angular2[2] = vel_angular2_eigen.z(); - acc_linear2[0] = acc_linear2_eigen.x(); - acc_linear2[1] = acc_linear2_eigen.y(); - acc_linear2[2] = acc_linear2_eigen.z(); - } + position2[0], + position2[1], + position2[2], + orientation2[0], + orientation2[1], + orientation2[2], + vel_linear2[0], + vel_linear2[1], + vel_linear2[2], + vel_angular2[0], + vel_angular2[1], + vel_angular2[2], + acc_linear2[0], + acc_linear2[1], + acc_linear2[2]); +} /** - * @brief Given a state and time delta, predicts a new state + computes the Jacobians - - * version for predicting new states inside the plugin - * @param[in] position - First position - * @param[in] orientation - First orientation - * @param[in] vel_linear - First linear velocity - * @param[in] vel_angular - First angular velocity - * @param[in] acc_linear - First linear acceleration + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation (quaternion) + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration * @param[in] dt - The time delta across which to predict the state - * @param[out] position_out - Second position - * @param[out] orientation_out - Second orientation - * @param[out] vel_linear_out - Second linear velocity - * @param[out] vel_angular_out - Second angular velocity - * @param[out] acc_linear_out - Second linear acceleration - * @param[out] state_tf_jacobian - Jacobian of the motion model + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation (quaternion) + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration */ inline void predict( const fuse_core::Vector3d & position1, @@ -336,9 +247,6 @@ inline void predict( fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) ); - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); // 3D material point projection model which matches the one used by r_l. double sr = ceres::sin(rpy.x()); double cr = ceres::cos(rpy.x()); @@ -348,9 +256,7 @@ inline void predict( double cy = ceres::cos(rpy.z()); double cpi = 1.0 / cp; - fuse_core::Matrix3d tf_pos, tf_rot, tf_vel, tf_acc; - fuse_core::Matrix15d state_tf; - + fuse_core::Matrix3d tf_pos, tf_rot; tf_pos(0, 0) = (cy * cp); tf_pos(0, 1) = (cy * sp * sr - sy * cr); tf_pos(0, 2) = (cy * sp * cr + sy * sr); @@ -372,7 +278,7 @@ inline void predict( tf_rot(2, 2) = cr * cpi; // Project the state - position2 = position1 + tf_pos * vel_linear1 * dt + 0.5 * tf_pos * acc_linear1 * dt * dt; + position2 = position1 + tf_pos * vel_linear1 * dt + tf_pos * acc_linear1 * 0.5 * dt * dt; rpy = rpy + tf_rot * vel_angular1 * dt; vel_linear2 = vel_linear1 + acc_linear1 * dt; vel_angular2 = vel_angular1; @@ -384,9 +290,9 @@ inline void predict( // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); -} + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + } } // namespace fuse_models #endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index 009af8d0f..f68253960 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -87,86 +87,87 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + // fuse_models::predict( + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); - q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + // q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); - EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); - EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_TRUE(q.isApprox(orientation2)); - EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); - EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + // EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + // EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + // EXPECT_DOUBLE_EQ(0.0, position2.z()); + // EXPECT_TRUE(q.isApprox(orientation2)); + // EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + // EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + // EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - // // Use non-zero Y values - vel_linear1.y() = -1.0; - vel_angular1.z() = -1.570796327; - acc_linear1.y() = -1.0; + // // // Use non-zero Y values + // vel_linear1.y() = -1.0; + // vel_angular1.z() = -1.570796327; + // acc_linear1.y() = -1.0; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + // fuse_models::predict( + // position1, + // orientation1, + // vel_linear1, + // vel_angular1, + // acc_linear1, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); - q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + // q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - EXPECT_DOUBLE_EQ(0.105, position2.x()); - EXPECT_DOUBLE_EQ(-0.105, position2.y()); - EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_TRUE(q.isApprox(orientation2)); - EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); - EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); - EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + // EXPECT_DOUBLE_EQ(0.105, position2.x()); + // EXPECT_DOUBLE_EQ(-0.105, position2.y()); + // EXPECT_DOUBLE_EQ(0.0, position2.z()); + // EXPECT_TRUE(q.isApprox(orientation2)); + // EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + // EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + // EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + // EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + // EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + // EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + // EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + // EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // TODO: continue with out of plane motion } TEST(Predict, predictFromDoublePointers) { - const double position1[3] {0.0, 0.0, 0.0}; - const double orientation1[4] {1.0, 0.0, 0.0, 0.0}; - const double vel_linear1[3] {1.0, 0.0, 0.0}; - const double vel_angular1[3] {0.0, 0.0, 1.570796327}; - const double acc_linear1[3] {1.0, 0.0, 0.0}; + double position1[3] {0.0, 0.0, 0.0}; + // double orientation1[4] {1.0, 0.0, 0.0, 0.0}; + double orientation1[3] {0.0, 0.0, 0.0}; + double vel_linear1[3] {1.0, 0.0, 0.0}; + double vel_angular1[3] {0.0, 0.0, 1.570796327}; + double acc_linear1[3] {1.0, 0.0, 0.0}; const double dt = 0.1; double position2[3] {1000.0, 0.0, 0.0}; - double orientation2[4]; + double orientation2[3]; double vel_linear2[3]; double vel_angular2[3]; double acc_linear2[3]; @@ -184,18 +185,12 @@ TEST(Predict, predictFromDoublePointers) vel_angular2, acc_linear2); - fuse_core::Quaternion q = - Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(0.0, position2[1]); EXPECT_DOUBLE_EQ(0.0, position2[2]); - EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); - EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); - EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); - EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); @@ -206,19 +201,19 @@ TEST(Predict, predictFromDoublePointers) EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - // // Carry on with the output state from last time - show in-place update support + // Carry on with the output state from last time - show in-place update support // fuse_models::predict( - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); // q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * @@ -247,17 +242,17 @@ TEST(Predict, predictFromDoublePointers) // acc_linear1[1] = -1.0; // fuse_models::predict( - // position1, - // orientation1, - // vel_linear1, - // vel_angular1, - // acc_linear1, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); + // position1, + // orientation1, + // vel_linear1, + // vel_angular1, + // acc_linear1, + // dt, + // position2, + // orientation2, + // vel_linear2, + // vel_angular2, + // acc_linear2); // q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * @@ -265,7 +260,7 @@ TEST(Predict, predictFromDoublePointers) // EXPECT_DOUBLE_EQ(0.105, position2[0]); // EXPECT_DOUBLE_EQ(-0.105,position2[1]); - // EXPECT_DOUBLE_EQ(0.0, position2[2]); + // EXPECT_DOUBLE_EQ(-0.105, position2[2]); // EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); // EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); // EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); @@ -279,7 +274,7 @@ TEST(Predict, predictFromDoublePointers) // EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); // EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); // EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - // // TODO: continue with out of plane motion + // TODO: continue with out of plane motion } TEST(Predict, predictFromJetPointers) @@ -287,13 +282,13 @@ TEST(Predict, predictFromJetPointers) using Jet = ceres::Jet; Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Jet orientation1[4] = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet orientation1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; const Jet dt = Jet(0.1); Jet position2[3]; - Jet orientation2[4]; + Jet orientation2[3]; Jet vel_linear2[3]; Jet vel_angular2[3]; Jet acc_linear2[3]; @@ -311,18 +306,12 @@ TEST(Predict, predictFromJetPointers) vel_angular2, acc_linear2); - Eigen::Quaternion q = - Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); @@ -334,328 +323,328 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); +// fuse_models::predict( +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); + +// q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); - EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); - EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - - // // Use non-zero Y values - vel_linear1[1] = Jet(-1.0); - vel_angular1[2] = Jet(-1.570796327); - acc_linear1[1] = Jet(-1.0); - - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); +// EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); +// EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); +// EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); +// EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); +// EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); +// EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); +// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); +// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// // // Use non-zero Y values +// vel_linear1[1] = Jet(-1.0); +// vel_angular1[2] = Jet(-1.570796327); +// acc_linear1[1] = Jet(-1.0); + +// fuse_models::predict( +// position1, +// orientation1, +// vel_linear1, +// vel_angular1, +// acc_linear1, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); - q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - - EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); - EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); +// q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + +// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); +// EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[2].a); +// EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); +// EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); +// EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); +// EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); +// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); +// EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); +// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // TODO: continue with out of plane motion } -TEST(Predict, predictFromEigenMatrixDouble) -{ - Eigen::Matrix position1 = {0.0, 0.0, 0.0}; - Eigen::Quaterniond orientation1 = {1.0, 0.0, 0.0, 0.0}; - Eigen::Matrix vel_linear1 = {1.0, 0.0, 0.0}; - Eigen::Matrix vel_angular1 = {0.0, 0.0, 1.570796327}; - Eigen::Matrix acc_linear1 = {1.0, 0.0, 0.0}; - const double dt = 0.1; - Eigen::Matrix position2; - Eigen::Quaterniond orientation2; - Eigen::Matrix vel_linear2; - Eigen::Matrix vel_angular2; - Eigen::Matrix acc_linear2; - - fuse_models::predict_eigen( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - fuse_core::Quaternion q = - Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - - EXPECT_DOUBLE_EQ(0.105, position2.x()); - EXPECT_DOUBLE_EQ(0.0, position2.y()); - EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); - EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); - EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); - EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); - EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); - EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - - // // Carry on with the output state from last time - show in-place update support - fuse_models::predict_eigen( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); +// TEST(Predict, predictFromEigenMatrixDouble) +// { +// Eigen::Matrix position1 = {0.0, 0.0, 0.0}; +// Eigen::Quaterniond orientation1 = {1.0, 0.0, 0.0, 0.0}; +// Eigen::Matrix vel_linear1 = {1.0, 0.0, 0.0}; +// Eigen::Matrix vel_angular1 = {0.0, 0.0, 1.570796327}; +// Eigen::Matrix acc_linear1 = {1.0, 0.0, 0.0}; +// const double dt = 0.1; +// Eigen::Matrix position2; +// Eigen::Quaterniond orientation2; +// Eigen::Matrix vel_linear2; +// Eigen::Matrix vel_angular2; +// Eigen::Matrix acc_linear2; + +// fuse_models::predict_eigen( +// position1, +// orientation1, +// vel_linear1, +// vel_angular1, +// acc_linear1, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); + +// fuse_core::Quaternion q = +// Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + +// EXPECT_DOUBLE_EQ(0.105, position2.x()); +// EXPECT_DOUBLE_EQ(0.0, position2.y()); +// EXPECT_DOUBLE_EQ(0.0, position2.z()); +// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); +// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); +// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); +// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); +// EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); +// EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); +// EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); +// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); +// EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + +// // // Carry on with the output state from last time - show in-place update support +// fuse_models::predict_eigen( +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); + +// q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); - EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); - EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); - EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); - EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); - EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); - EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); - EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - - // // Use non-zero Y values - vel_linear1[1] = -1.0; - vel_angular1[2] = -1.570796327; - acc_linear1[1] = -1.0; - - fuse_models::predict_eigen( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); +// EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); +// EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); +// EXPECT_DOUBLE_EQ(0.0, position2.z()); +// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); +// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); +// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); +// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); +// EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); +// EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); +// EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); +// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); +// EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + +// // // Use non-zero Y values +// vel_linear1[1] = -1.0; +// vel_angular1[2] = -1.570796327; +// acc_linear1[1] = -1.0; + +// fuse_models::predict_eigen( +// position1, +// orientation1, +// vel_linear1, +// vel_angular1, +// acc_linear1, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); - q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - - EXPECT_DOUBLE_EQ(0.105, position2.x()); - EXPECT_DOUBLE_EQ(-0.105,position2.y()); - EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); - EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); - EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); - EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); - EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); - EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); - EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); - EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - // TODO: continue with out of plane motion -} - -TEST(Predict, predictFromEigenMatrixJet) -{ - using Jet = ceres::Jet; - Eigen::Matrix position1 = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Eigen::Quaternion orientation1 = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; - Eigen::Matrix vel_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; - Eigen::Matrix vel_angular1 = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; - Eigen::Matrix acc_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; - const Jet dt = Jet(0.1); - Eigen::Matrix position2; - Eigen::Quaternion orientation2; - Eigen::Matrix vel_linear2; - Eigen::Matrix vel_angular2; - Eigen::Matrix acc_linear2; - - fuse_models::predict_eigen( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - Eigen::Quaternion q = - Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - - EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); - EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - - // // Carry on with the output state from last time - show in-place update support - fuse_models::predict_eigen( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); +// q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + +// EXPECT_DOUBLE_EQ(0.105, position2.x()); +// EXPECT_DOUBLE_EQ(-0.105,position2.y()); +// EXPECT_DOUBLE_EQ(0.0, position2.z()); +// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); +// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); +// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); +// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); +// EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); +// EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); +// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); +// EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); +// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); +// EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); +// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); +// // TODO: continue with out of plane motion +// } + +// TEST(Predict, predictFromEigenMatrixJet) +// { +// using Jet = ceres::Jet; +// Eigen::Matrix position1 = {Jet(0.0), Jet(0.0), Jet(0.0)}; +// Eigen::Quaternion orientation1 = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; +// Eigen::Matrix vel_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; +// Eigen::Matrix vel_angular1 = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; +// Eigen::Matrix acc_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; +// const Jet dt = Jet(0.1); +// Eigen::Matrix position2; +// Eigen::Quaternion orientation2; +// Eigen::Matrix vel_linear2; +// Eigen::Matrix vel_angular2; +// Eigen::Matrix acc_linear2; + +// fuse_models::predict_eigen( +// position1, +// orientation1, +// vel_linear1, +// vel_angular1, +// acc_linear1, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); + +// Eigen::Quaternion q = +// Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + +// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); +// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); +// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); +// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); +// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); +// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); +// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); +// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// // // Carry on with the output state from last time - show in-place update support +// fuse_models::predict_eigen( +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); + +// q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); - EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); - EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - - // // Use non-zero Y values - vel_linear1[1] = Jet(-1.0); - vel_angular1[2] = Jet(-1.570796327); - acc_linear1[1] = Jet(-1.0); - - fuse_models::predict_eigen( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); +// EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); +// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); +// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); +// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); +// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); +// EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); +// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); +// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// // // Use non-zero Y values +// vel_linear1[1] = Jet(-1.0); +// vel_angular1[2] = Jet(-1.570796327); +// acc_linear1[1] = Jet(-1.0); + +// fuse_models::predict_eigen( +// position1, +// orientation1, +// vel_linear1, +// vel_angular1, +// acc_linear1, +// dt, +// position2, +// orientation2, +// vel_linear2, +// vel_angular2, +// acc_linear2); - q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * - Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - - EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); - EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); - EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); - EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); - EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); - EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - // TODO: continue with out of plane motion -} \ No newline at end of file +// q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * +// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); + +// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); +// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); +// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); +// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); +// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); +// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); +// EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); +// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); +// EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); +// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); +// // TODO: continue with out of plane motion +// } \ No newline at end of file From d3e3c36fc2e305f3724b0147b1056dc19243bf83 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 14:02:51 +0000 Subject: [PATCH 036/116] unicycle 3d cost functor refactor + test --- .../unicycle_3d_state_cost_functor.hpp | 68 +++++++++---------- .../test_unicycle_3d_state_cost_function.cpp | 29 ++++---- 2 files changed, 48 insertions(+), 49 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index 9d219e8f8..a822c7ba7 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -147,14 +147,28 @@ bool Unicycle3DStateCostFunctor::operator()( const T * const acc_linear2, T * residual) const { + // Initialize predicted state variables T position_pred[3] {T(0.0)}; - T orientation_pred[4] {T(0.0)}; // storage order: w, x, y, z + T orientation_pred[3] {T(0.0)}; T vel_linear_pred[3] {T(0.0)}; T vel_angular_pred[3] {T(0.0)}; T acc_linear_pred[3] {T(0.0)}; + + // Convert orientation variables from quaternion to roll-pitch-yaw + const T orientation1_rpy[3] { + fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) + }; + const T orientation2_rpy[3] { + fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) + }; + predict( position1, - orientation1, + orientation1_rpy, vel_linear1, vel_angular1, acc_linear1, @@ -164,41 +178,23 @@ bool Unicycle3DStateCostFunctor::operator()( vel_linear_pred, vel_angular_pred, acc_linear_pred); - - Eigen::Map> residuals_map(residual); - T orientation_pred_rpy[3]; - T orientation2_rpy[3]; - - orientation_pred_rpy[0] = - fuse_core::getRoll(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); - orientation_pred_rpy[1] = - fuse_core::getPitch(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); - orientation_pred_rpy[2] = - fuse_core::getYaw(orientation_pred[0], orientation_pred[1], orientation_pred[2], orientation_pred[3]); - - orientation2_rpy[0] = - fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); - orientation2_rpy[1] = - fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); - orientation2_rpy[2] = - fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); - - residuals_map(0) = T(position2[0] - position_pred[0]); - residuals_map(1) = T(position2[1] - position_pred[1]); - residuals_map(2) = T(position2[2] - position_pred[2]); - residuals_map(3) = T(orientation2_rpy[0] - orientation_pred_rpy[0]); - residuals_map(4) = T(orientation2_rpy[1] - orientation_pred_rpy[1]); - residuals_map(5) = T(orientation2_rpy[2] - orientation_pred_rpy[2]); - residuals_map(6) = T(vel_linear2[0] - vel_linear_pred[0]); - residuals_map(7) = T(vel_linear2[1] - vel_linear_pred[1]); - residuals_map(8) = T(vel_linear2[2] - vel_linear_pred[2]); - residuals_map(9) = T(vel_angular2[0] - vel_angular_pred[0]); - residuals_map(10) = T(vel_angular2[1] - vel_angular_pred[1]); - residuals_map(11) = T(vel_angular2[2] - vel_angular_pred[2]); - residuals_map(12) = T(acc_linear2[0] - acc_linear_pred[0]); - residuals_map(13) = T(acc_linear2[1] - acc_linear_pred[1]); - residuals_map(14) = T(acc_linear2[2] - acc_linear_pred[2]); + Eigen::Map> residuals_map(residual); + residuals_map(0) = position2[0] - position_pred[0]; + residuals_map(1) = position2[1] - position_pred[1]; + residuals_map(2) = position2[2] - position_pred[2]; + residuals_map(3) = orientation2_rpy[0] - orientation_pred[0]; + residuals_map(4) = orientation2_rpy[1] - orientation_pred[1]; + residuals_map(5) = orientation2_rpy[2] - orientation_pred[2]; + residuals_map(6) = vel_linear2[0] - vel_linear_pred[0]; + residuals_map(7) = vel_linear2[1] - vel_linear_pred[1]; + residuals_map(8) = vel_linear2[2] - vel_linear_pred[2]; + residuals_map(9) = vel_angular2[0] - vel_angular_pred[0]; + residuals_map(10) = vel_angular2[1] - vel_angular_pred[1]; + residuals_map(11) = vel_angular2[2] - vel_angular_pred[2]; + residuals_map(12) = acc_linear2[0] - acc_linear_pred[0]; + residuals_map(13) = acc_linear2[1] - acc_linear_pred[1]; + residuals_map(14) = acc_linear2[2] - acc_linear_pred[2]; fuse_core::wrapAngle2D(residuals_map(3)); fuse_core::wrapAngle2D(residuals_map(4)); diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp index c9744c768..4ace46cea 100644 --- a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -59,24 +59,27 @@ TEST(CostFunction, evaluateCostFunction) // Evaluate cost function const double position1[] = {0.0, 0.0, 0.0}; - const double orientation1[] = {0.0, 0.0, 0.0}; - const double vel_linear1[] = {1.0, 0.0}; - const double vel_yaw1[] = {1.570796327}; - const double acc_linear1[] = {1.0, 0.0}; - - const double position2[] = {0.105, 0.0}; - const double orientation2[] = {0.1570796327}; - const double vel_linear2[] = {1.1, 0.0}; - const double vel_yaw2[] = {1.570796327}; - const double acc_linear2[] = {1.0, 0.0}; + const double orientation1[] = {1.0, 0.0, 0.0, 0.0}; + const double vel_linear1[] = {1.0, 0.0, 0.0}; + const double vel_angular1[] = {0.0, 0.0, 1.570796327}; + const double acc_linear1[] = {1.0, 0.0, 0.0}; + + const double position2[] = {0.105, 0.0, 0.0}; + Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + const double orientation2[] = {q2.w(), q2.x(), q2.y(), q2.z()}; + const double vel_linear2[] = {1.1, 0.0, 0.0}; + const double vel_angular2[] = {0.0, 0.0, 1.570796327}; + const double acc_linear2[] = {1.0, 0.0, 0.0}; const double * parameters[] = { - position1, orientation1, vel_linear1, vel_yaw1, acc_linear1, - position2, orientation2, vel_linear2, vel_yaw2, acc_linear2 + position1, orientation1, vel_linear1, vel_angular1, acc_linear1, + position2, orientation2, vel_linear2, vel_angular2, acc_linear2 }; - fuse_core::Vector8d residuals; + fuse_core::Vector15d residuals; // const auto & block_sizes = cost_function.parameter_block_sizes(); // const auto num_parameter_blocks = block_sizes.size(); From c44aa4e63d7157280954bed9a7d082a43cfb0621 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 14:05:04 +0000 Subject: [PATCH 037/116] different handling for orient/vel variables in sensor config --- .../fuse_models/common/sensor_config.hpp | 55 +++---------------- .../fuse_models/common/variable_traits.hpp | 16 +++++- 2 files changed, 22 insertions(+), 49 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index 6967ec6b3..d9eeb4b5b 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -148,17 +148,17 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +std::enable_if_t::value && !is_orientation::value, size_t> toIndex(const std::string & dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "roll" || lower_dim == "x") { - return static_cast(T::Euler::ROLL) - 4UL; + return static_cast(T::ROLL); } if (lower_dim == "pitch" || lower_dim == "y") { - return static_cast(T::Euler::PITCH) - 4UL; + return static_cast(T::PITCH); } if (lower_dim == "yaw" || lower_dim == "z") { - return static_cast(T::Euler::YAW) - 4UL; + return static_cast(T::YAW); } throwDimensionError(dimension); @@ -166,27 +166,19 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di return 0u; } -/** - * @brief Method that converts from 3D angular axis dimension names to index values - * - * This method is enabled only for variables that contain _only_ 3D angular quantities - * - * @param[in] dimension - The dimension name to convert - * @return the index of the enumerated dimension for that type - * @throws runtime_error if the dimension name is invalid - */ template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +std::enable_if_t::value && is_orientation::value, size_t> toIndex(const std::string & dimension) { + // Trick to get roll, pitch, yaw indexes as 0, 1, 2 auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "roll" || lower_dim == "x") { - return static_cast(T::ROLL); + return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 4UL; } if (lower_dim == "pitch" || lower_dim == "y") { - return static_cast(T::PITCH); + return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 4UL; } if (lower_dim == "yaw" || lower_dim == "z") { - return static_cast(T::YAW); + return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 4UL; } throwDimensionError(dimension); @@ -194,35 +186,6 @@ std::enable_if_t::value, size_t> toIndex(const std::string return 0u; } -// /** -// * @brief Method that converts from 3D angular axis dimension names to index values -// * -// * This method is enabled only for variable Orientation3DStamped -// * -// * @param[in] dimension - The dimension name to convert -// * @return the index of the enumerated dimension for that type -// * @throws runtime_error if the dimension name is invalid -// */ -// template<> -// std::enable_if_t::value, size_t> toIndex(const std::string & dimension) -// { -// auto lower_dim = boost::algorithm::to_lower_copy(dimension); -// if (lower_dim == "roll" || lower_dim == "x") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 3UL; -// // Trick to obtain indices of roll, pitch, yaw as 3, 4, 5 relative to 3d pose_mean -// } -// if (lower_dim == "pitch" || lower_dim == "y") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 3UL; -// } -// if (lower_dim == "yaw" || lower_dim == "z") { -// return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 3UL; -// } - -// throwDimensionError(dimension); - -// return 0u; -// } - /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index 6ff7b965a..19eef89e8 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -128,17 +128,27 @@ struct is_angular_3d { static const bool value = true; }; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; + template -struct is_angular_vel_3d +struct is_orientation { static const bool value = false; }; template<> -struct is_angular_vel_3d +struct is_orientation +{ + static const bool value = true; +}; +template<> +struct is_orientation { static const bool value = true; }; - } // namespace common } // namespace fuse_models From 569d8ffcf35ef13ecad0f205e74768ae4066158c Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 14:05:25 +0000 Subject: [PATCH 038/116] remove TODOs --- fuse_models/src/unicycle_3d.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index f7f358f91..9ca2a1005 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -89,7 +89,6 @@ std::string to_string(const fuse_core::Quaternion & quaternion) oss << quaternion; return oss.str(); } - } // namespace std namespace fuse_core @@ -217,7 +216,6 @@ void Unicycle3D::onInit() process_noise_covariance_ = fuse_core::Vector15d(process_noise_diagonal.data()).asDiagonal(); - // TODO: check these parameters scale_process_noise_ = fuse_core::getParam( interfaces_, fuse_core::joinParameterName( @@ -301,7 +299,6 @@ void Unicycle3D::generateMotionModel( } StateHistoryElement state1; - // If the nearest state we had was before the beginning stamp, we need to project that state to // the beginning stamp if (base_time != beginning_stamp) { @@ -320,7 +317,7 @@ void Unicycle3D::generateMotionModel( } else { state1 = base_state; } - + // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); @@ -352,7 +349,7 @@ void Unicycle3D::generateMotionModel( state2.vel_angular, state2.acc_linear); - // Define the fuse variables required for this constraint + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( @@ -402,7 +399,7 @@ void Unicycle3D::generateMotionModel( position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); - + orientation2->data()[fuse_variables::Orientation3DStamped::X] = state2.orientation.x(); orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); @@ -528,13 +525,13 @@ void Unicycle3D::updateStateHistoryEstimates( { const auto & current_stamp = current_iter->first; auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.orientation_uuid) && graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_angular_uuid) && graph.variableExists(current_state.acc_linear_uuid)) { - // This pose does exist in the graph. Update it directly. + // This pose does exist in the graph. Update it directly. const auto & position = graph.getVariable(current_state.position_uuid); const auto & orientation = graph.getVariable(current_state.orientation_uuid); const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); @@ -574,7 +571,7 @@ void Unicycle3D::updateStateHistoryEstimates( auto previous_iter = std::prev(current_iter); const auto & previous_stamp = previous_iter->first; const auto & previous_state = previous_iter->second; - + // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction @@ -610,7 +607,6 @@ void Unicycle3D::validateMotionModel( } catch (const std::runtime_error & ex) { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } - // TODO: check validate covariance for 15x15 matrixes try { fuse_core::validateCovariance(process_noise_covariance); } catch (const std::runtime_error & ex) { From bd30458dc84fba5af5ca47d9d8359c57bb13876a Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 28 Nov 2023 14:06:22 +0000 Subject: [PATCH 039/116] abs and diff pose handling --- .../fuse_models/common/sensor_proc.hpp | 179 +++++++++++------- 1 file changed, 115 insertions(+), 64 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 9e0f6a7b5..2f315da20 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -76,6 +76,7 @@ #include #include +#include "covariance_geometry/pose_composition.hpp" #include "covariance_geometry/pose_covariance_representation.hpp" #include "covariance_geometry/pose_covariance_composition.hpp" #include "covariance_geometry_ros/utils.hpp" @@ -178,7 +179,6 @@ inline std::vector mergeIndices( return merged_indices; } -// TODO: is possible to enlarge this to work with eigen maps and view of those? /** * @brief Method to create sub-measurements from full measurements and append them to existing * partial measurements @@ -498,20 +498,27 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - // Convert the ROS message into tf2 transform tf2::Transform tf2_pose; tf2::fromMsg(transformed_message.pose.pose, tf2_pose); + // Create the pose variable + auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); + position->x() = tf2_pose.getOrigin().x(); + position->y() = tf2_pose.getOrigin().y(); + position->z() = tf2_pose.getOrigin().z(); + auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + orientation->x() = tf2_pose.getRotation().x(); + orientation->y() = tf2_pose.getRotation().y(); + orientation->z() = tf2_pose.getRotation().z(); + orientation->w() = tf2_pose.getRotation().w(); + // Fill eigen pose in RPY representation fuse_core::Vector6d pose_mean; - pose_mean(0) = tf2_pose.getOrigin().x(); - pose_mean(1) = tf2_pose.getOrigin().y(); - pose_mean(2) = tf2_pose.getOrigin().z(); - tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( - pose_mean(3), pose_mean(4), pose_mean(5)); - - Eigen::Map pose_covariance_map(transformed_message.pose.covariance.data()); + pose_mean.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean(3), pose_mean(4), pose_mean(5)); + + Eigen::Map pose_covariance_map(transformed_message.pose.covariance.data()); // Set the components which are not measured to zero const auto indices = mergeIndices(position_indices, orientation_indices, 3); @@ -526,18 +533,6 @@ inline bool processAbsolutePose3DWithCovariance( tf2::Quaternion q_partial; q_partial.setRPY(pose_mean(3), pose_mean(4), pose_mean(5)); - - // Create the pose variable - auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); - auto orientation = - fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); - position->x() = pose_mean(0); - position->y() = pose_mean(1); - position->z() = pose_mean(2); - orientation->x() = q_partial.getX(); - orientation->y() = q_partial.getY(); - orientation->z() = q_partial.getZ(); - orientation->w() = q_partial.getW(); // Create the pose for the constraint fuse_core::Vector7d pose_mean_partial; @@ -958,6 +953,7 @@ inline bool processDifferentialPoseWithCovariance( * @param[in] device_id - The UUID of the machine * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] independent - Whether the pose measurements are indepent or not * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always * added to the resulting pose relative covariance * @param[in] loss - The loss function for the 2D pose constraint generated @@ -973,6 +969,7 @@ inline bool processDifferentialPose3DWithCovariance( const fuse_core::UUID & device_id, const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, + const bool independent, const fuse_core::Matrix6d & minimum_pose_relative_covariance, const fuse_core::Loss::SharedPtr & loss, const std::vector & position_indices, @@ -980,30 +977,106 @@ inline bool processDifferentialPose3DWithCovariance( const bool validate, fuse_core::Transaction & transaction) { - // Pipeline to consider only the measured positions and orientations - // Convert the ROS message into tf2 transform - // Fill eigen pose in RPY representation - // Set the components which are not measured to zero - // Create the pose variable for the graph - // Create the pose for the constraint (in quaternion) - // Create the constraint + // TODO:: we should probably remove covariance_geometry dependency // Convert from ROS msg to covariance geometry types // PoseQuaternionCovarianceRPY is std::pair, Covariance> // Position is Eigen::Vector3d // Quaternion is Eigen::Quaterniond // Covariance is Eigen::Matrix6d - // TODO: implement a fromROS method which outputs PoseRPYCovariance directly covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; covariance_geometry::fromROS(pose1.pose, p1); covariance_geometry::fromROS(pose2.pose, p2); - // Create the delta for the constraint and the relative covariance - covariance_geometry::ComposePoseQuaternionCovarianceRPY( - covariance_geometry::inversePose3DQuaternionCovarianceRPY(p1), - p2, - p12); +// Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = p1.first.first.x(); + position1->y() = p1.first.first.y(); + position1->z() = p1.first.first.z(); + orientation1->x() = p1.first.second.x(); + orientation1->y() = p1.first.second.y(); + orientation1->z() = p1.first.second.z(); + orientation1->w() = p1.first.second.w(); + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = p2.first.first.x(); + position2->y() = p2.first.first.y(); + position2->z() = p2.first.first.z(); + orientation2->x() = p2.first.second.x(); + orientation2->y() = p2.first.second.y(); + orientation2->z() = p2.first.second.z(); + orientation2->w() = p2.first.second.w(); + + // Create the delta for the constraint + if (independent) { + covariance_geometry::ComposePoseQuaternionCovarianceRPY( + covariance_geometry::inversePose3DQuaternionCovarianceRPY(p1), + p2, + p12 + ); + } else { + // TODO: check this method, results are nosense + // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. + // We know cov1 and cov2 and we should substract the first to the second in order + // to obtain the relative pose covariance. But first the 2 of them have to be in the + // same reference frame, moreover we need to rotate the result in p12 reference frame + // The covariance propagation of p2 = p1 * p12 is: + // + // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T + // + // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and + // J_p12 are the jacobians of the equation (pose composition) wrt p1 and p12, respectively. + // + // Therefore, the covariance C12 of the relative pose p12 is: + // + // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T + + // First we need to convert covariances from RPY(6x6) to Quat(7x7) + covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p1, + p1_q + ); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p2, + p2_q + ); + // Then we need to compute the delta pose + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1_q.first), + p2_q.first, + p12_q.first + ); + // Now we have to compute pose composition jacobians so we can rotate covariances + Eigen::Matrix7d j_p1; + Eigen::Matrix7d j_p12; + Eigen::Matrix4d j_qn; + j_p1.setZero(); + j_p12.setZero(); + j_qn.setZero(); + covariance_geometry::jacobianQuaternionNormalization(p2_q.first.second, j_qn); + covariance_geometry::JacobianPosePoseCompositionA(p1_q.first, p12_q.first, j_p1); + j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + covariance_geometry::JacobianPosePoseCompositionB(p1_q.first, j_p12); + j_p12.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + Eigen::Matrix7d j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); + p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * + j_p12_inv.transpose(); + + // Now again convert the delta pose back to covariance in RPY(6x6) + covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( + p12_q, + p12 + ); + } + // std::cout << "Relative pose: " << std::endl; + // std::cout << " Position: " << p12.first.first.transpose() << std::endl; + // std::cout << " Rotation: " << p12.first.second << std::endl; + // std::cout << " Covariance: " << p12.second << std::endl; // Convert the poses into RPY representation covariance_geometry::PoseRPYCovariance p1_rpy, p2_rpy, p12_rpy; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( @@ -1012,7 +1085,8 @@ inline bool processDifferentialPose3DWithCovariance( p2, p2_rpy); covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( p12, p12_rpy); - + + p12_rpy.second += minimum_pose_relative_covariance; // Set the components which are not measured to zero // p1_partial std::replace_if( @@ -1079,29 +1153,6 @@ inline bool processDifferentialPose3DWithCovariance( covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( p12_rpy, p12_partial); - // Create the pose variables - auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); - auto orientation1 = - fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); - position1->x() = p1_partial.first.first.x(); - position1->y() = p1_partial.first.first.y(); - position1->z() = p1_partial.first.first.z(); - orientation1->x() = p1_partial.first.second.x(); - orientation1->y() = p1_partial.first.second.y(); - orientation1->z() = p1_partial.first.second.z(); - orientation1->w() = p1_partial.first.second.w(); - - auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = - fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); - position2->x() = p2_partial.first.first.x(); - position2->y() = p2_partial.first.first.y(); - position2->z() = p2_partial.first.first.z(); - orientation2->x() = p2_partial.first.second.x(); - orientation2->y() = p2_partial.first.second.y(); - orientation2->z() = p2_partial.first.second.z(); - orientation2->w() = p2_partial.first.second.w(); - // Create mean vector and partial covariance matrix for the constraint fuse_core::Vector7d pose_relative_mean; pose_relative_mean << @@ -1112,7 +1163,7 @@ inline bool processDifferentialPose3DWithCovariance( const auto indices = mergeIndices(position_indices, orientation_indices, 3); fuse_core::MatrixXd pose_relative_covariance(indices.size(), indices.size()); populatePartialMeasurement( - p12_partial.second + minimum_pose_relative_covariance, + p12_partial.second, indices, pose_relative_covariance); @@ -1595,13 +1646,13 @@ inline bool processTwist3DWithCovariance( transformed_message.twist.twist.linear.z; // Create the covariance for the constraint - // TODO check if this the correct way for not doing copies + // TODO: check how to only map linear 3x3 part of the covariance Eigen::Map linear_vel_covariance_map( - transformed_message.twist.covariance.data()) ; - // fuse_core::Matrix3d linear_vel_covariance = cov_map.block<3, 3>(0, 0); + transformed_message.twist.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), linear_vel_mean_partial.rows()); @@ -1654,8 +1705,8 @@ inline bool processTwist3DWithCovariance( transformed_message.twist.twist.angular.z; // Create the covariance for the constraint + // TODO: check how to only map angular 3x3 part of the covariance Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance.data()); - // fuse_core::Matrix3d angular_vel_covariance = cov_map.block<3,3>(3,3); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); @@ -1877,7 +1928,7 @@ inline bool processAccel3DWithCovariance( transformed_message.accel.accel.linear.y, transformed_message.accel.accel.linear.z; - Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); + Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); From 04a984d9e21ef008a2913b4a977acab3276ce6ca Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 5 Dec 2023 14:26:46 +0000 Subject: [PATCH 040/116] unicycle 3d tests + minor fixes --- ...st_absolute_pose_3d_stamped_constraint.cpp | 25 +- fuse_models/src/unicycle_3d.cpp | 6 +- fuse_models/test/CMakeLists.txt | 1 + fuse_models/test/test_unicycle_3d.cpp | 400 ++++++++ fuse_models/test/test_unicycle_3d_predict.cpp | 939 +++++++++--------- 5 files changed, 884 insertions(+), 487 deletions(-) create mode 100644 fuse_models/test/test_unicycle_3d.cpp diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index 990a2514d..f4841c0a3 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -176,29 +176,24 @@ TEST(AbsolutePose3DStampedConstraint, CovariancePartial) mean, cov, variable_indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') - // TODO: check if this is correct fuse_core::Matrix expected_sqrt_info; /* *INDENT-OFF* */ - // expected_sqrt_info << 2.12658752275893, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - // 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - // 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - // 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - // 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT - - expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, - 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, - 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, - 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, - 0. , 0.0, 0. , 0. , 0. , 0.68147774; + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT + 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT + 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT + 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; - + /* *INDENT-OFF* */ expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ // Compare EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index 9ca2a1005..8deb94af0 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -531,7 +531,7 @@ void Unicycle3D::updateStateHistoryEstimates( graph.variableExists(current_state.vel_angular_uuid) && graph.variableExists(current_state.acc_linear_uuid)) { - // This pose does exist in the graph. Update it directly. + // This pose does exist in the graph. Update it directly. const auto & position = graph.getVariable(current_state.position_uuid); const auto & orientation = graph.getVariable(current_state.orientation_uuid); const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); @@ -539,8 +539,8 @@ void Unicycle3D::updateStateHistoryEstimates( const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; - current_state.position.x() = position.data()[fuse_variables::Position3DStamped::Y]; - current_state.position.x() = position.data()[fuse_variables::Position3DStamped::Z]; + current_state.position.y() = position.data()[fuse_variables::Position3DStamped::Y]; + current_state.position.z() = position.data()[fuse_variables::Position3DStamped::Z]; current_state.orientation.x() = orientation.data()[fuse_variables::Orientation3DStamped::X]; current_state.orientation.y() = orientation.data()[fuse_variables::Orientation3DStamped::Y]; diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index c74b2b353..1cbb64482 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,6 +5,7 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition + test_unicycle_3d test_unicycle_3d_predict test_unicycle_3d_state_cost_function ) diff --git a/fuse_models/test/test_unicycle_3d.cpp b/fuse_models/test/test_unicycle_3d.cpp new file mode 100644 index 000000000..7e1052f42 --- /dev/null +++ b/fuse_models/test/test_unicycle_3d.cpp @@ -0,0 +1,400 @@ +/*************************************************************************** + * Copyright (C) 2017 Locus Robotics. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Derived class used in unit tests to expose protected functions + */ +class Unicycle3DModelTest : public fuse_models::Unicycle3D +{ +public: + using fuse_models::Unicycle3D::updateStateHistoryEstimates; + using fuse_models::Unicycle3D::StateHistoryElement; + using fuse_models::Unicycle3D::StateHistory; +}; + +TEST(Unicycle3D, UpdateStateHistoryEstimates) +{ + // Create some variables + auto position1 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0)); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + position1->x() = 1.1; + position1->y() = 2.1; + position1->z() = 0.0; + orientation1->w() = 1.0; + orientation1->x() = 0.0; + orientation1->y() = 0.0; + orientation1->z() = 0.0; + linear_velocity1->x() = 1.0; + linear_velocity1->y() = 0.0; + linear_velocity1->z() = 0.0; + angular_velocity1->roll() = 0.0; + angular_velocity1->pitch() = 0.0; + angular_velocity1->yaw() = 0.0; + linear_acceleration1->x() = 1.0; + linear_acceleration1->y() = 0.0; + linear_acceleration1->z() = 0.0; + auto position2 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(2, 0)); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + position2->x() = 1.2; + position2->y() = 2.2; + position2->z() = 0.0; + orientation2->w() = 1.0; + orientation2->x() = 0.0; + orientation2->y() = 0.0; + orientation2->z() = 0.0; + linear_velocity2->x() = 0.0; + linear_velocity2->y() = 1.0; + linear_velocity2->z() = 0.0; + angular_velocity2->roll() = 0.0; + angular_velocity2->pitch() = 0.0; + angular_velocity2->yaw() = 0.0; + linear_acceleration2->x() = 0.0; + linear_acceleration2->y() = 1.0; + linear_acceleration2->z() = 0.0; + auto position3 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(3, 0)); + auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + position3->x() = 1.3; + position3->y() = 2.3; + position3->z() = 0.0; + orientation3->w() = 1.0; + orientation3->x() = 0.0; + orientation3->y() = 0.0; + orientation3->z() = 0.0; + linear_velocity3->x() = 4.3; + linear_velocity3->y() = 5.3; + linear_velocity3->z() = 0.0; + angular_velocity3->roll() = 0.0; + angular_velocity3->pitch() = 0.0; + angular_velocity3->yaw() = 0.0; + linear_acceleration3->x() = 7.3; + linear_acceleration3->y() = 8.3; + linear_acceleration3->z() = 0.0; + auto position4 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(4, 0)); + auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + position4->x() = 1.4; + position4->y() = 2.4; + position4->z() = 0.0; + orientation4->w() = 1.0; + orientation4->x() = 0.0; + orientation4->y() = 0.0; + orientation4->z() = 0.0; + linear_velocity4->x() = 4.4; + linear_velocity4->y() = 5.4; + linear_velocity4->z() = 0.0; + angular_velocity4->roll() = 0.0; + angular_velocity4->pitch() = 0.0; + angular_velocity4->yaw() = 0.0; + linear_acceleration4->x() = 7.4; + linear_acceleration4->y() = 8.4; + linear_acceleration4->z() = 0.0; + auto position5 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(5, 0)); + auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + position5->x() = 1.5; + position5->y() = 2.5; + position5->z() = 0.0; + orientation5->w() = 1.0; + orientation5->x() = 0.0; + orientation5->y() = 0.0; + orientation5->z() = 0.0; + linear_velocity5->x() = 4.5; + linear_velocity5->y() = 5.5; + linear_velocity5->z() = 0.0; + angular_velocity5->roll() = 0.0; + angular_velocity5->pitch() = 0.0; + angular_velocity5->yaw() = 0.0; + linear_acceleration5->x() = 7.5; + linear_acceleration5->y() = 8.5; + linear_acceleration5->z() = 0.0; + + // Add a subset of the variables to a graph + fuse_graphs::HashGraph graph; + graph.addVariable(position2); + graph.addVariable(orientation2); + graph.addVariable(linear_velocity2); + graph.addVariable(angular_velocity2); + graph.addVariable(linear_acceleration2); + + graph.addVariable(position4); + graph.addVariable(orientation4); + graph.addVariable(linear_velocity4); + graph.addVariable(angular_velocity4); + graph.addVariable(linear_acceleration4); + + // Add all of the variables to the state history + Unicycle3DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + orientation1->uuid(), + linear_velocity1->uuid(), + angular_velocity1->uuid(), + linear_acceleration1->uuid(), + fuse_core::Vector3d(1.0, 0.0, 0.0), + fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position2->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position2->uuid(), + orientation2->uuid(), + linear_velocity2->uuid(), + angular_velocity2->uuid(), + linear_acceleration2->uuid(), + fuse_core::Vector3d(2.0, 0.0, 0.0), + fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position3->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position3->uuid(), + orientation3->uuid(), + linear_velocity3->uuid(), + angular_velocity3->uuid(), + linear_acceleration3->uuid(), + fuse_core::Vector3d(3.0, 0.0, 0.0), + fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position4->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position4->uuid(), + orientation4->uuid(), + linear_velocity4->uuid(), + angular_velocity4->uuid(), + linear_acceleration4->uuid(), + fuse_core::Vector3d(4.0, 0.0, 0.0), + fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position5->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position5->uuid(), + orientation5->uuid(), + linear_velocity5->uuid(), + angular_velocity5->uuid(), + linear_acceleration5->uuid(), + fuse_core::Vector3d(5.0, 0.0, 0.0), + fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + + // Update the state history + Unicycle3DModelTest::updateStateHistoryEstimates( + graph, state_history, rclcpp::Duration::from_seconds( + 10.0)); + + // Check the state estimates in the state history + { + // The first entry is missing from the graph. It will not get updated. + auto expected_position = fuse_core::Vector3d(1.0, 0.0, 0.0); + auto expected_orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(1, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(1, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(1, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(1, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(1, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The second entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph + auto expected_orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(2, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(2, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(2, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(2, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The third entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(3, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(3, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(3, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(3, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The forth entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(4, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(4, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(4.4, 5.4, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(4, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(4, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(4, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The fifth entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(5, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(5, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(11.8, 13.8, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(5, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } +} diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index f68253960..a247296ce 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -48,7 +48,7 @@ TEST(Predict, predictDirectVals) fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); - const double dt = 0.1; + double dt = 0.1; fuse_core::Vector3d position2; fuse_core::Quaternion orientation2; fuse_core::Vector3d vel_linear2; @@ -68,14 +68,18 @@ TEST(Predict, predictDirectVals) vel_angular2, acc_linear2); - fuse_core::Quaternion q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + fuse_core::Quaternion q; + q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); EXPECT_DOUBLE_EQ(0.0, position2.y()); EXPECT_DOUBLE_EQ(0.0, position2.z()); - EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); @@ -87,86 +91,164 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // // Carry on with the output state from last time - show in-place update support - // fuse_models::predict( - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); - - // q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - // EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); - // EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); - // EXPECT_DOUBLE_EQ(0.0, position2.z()); - // EXPECT_TRUE(q.isApprox(orientation2)); - // EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - // EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); - // EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - - // // // Use non-zero Y values - // vel_linear1.y() = -1.0; - // vel_angular1.z() = -1.570796327; - // acc_linear1.y() = -1.0; - - // fuse_models::predict( - // position1, - // orientation1, - // vel_linear1, - // vel_angular1, - // acc_linear1, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Use non-zero Y values + vel_linear1.y() = -1.0; + vel_angular1.z() = -1.570796327; + acc_linear1.y() = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); - // q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - - // EXPECT_DOUBLE_EQ(0.105, position2.x()); - // EXPECT_DOUBLE_EQ(-0.105, position2.y()); - // EXPECT_DOUBLE_EQ(0.0, position2.z()); - // EXPECT_TRUE(q.isApprox(orientation2)); - // EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); - // EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); - // EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); - // EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); - // EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - - // TODO: continue with out of plane motion + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(-0.105, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Out of plane motion + position1 = {0.0, 0.0, 0.0}; + orientation1 = {1.0, 0.0, 0.0, 0.0}; + vel_linear1 = {0.0, 0.0, 0.1}; + vel_angular1 = {1.570796327, 0.0, 0.0}; + acc_linear1 = {0.0, 0.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.015, position2.z()); + EXPECT_DOUBLE_EQ(0.99691733373232339, orientation2.w()); + EXPECT_DOUBLE_EQ(0.078459095738068516, orientation2.x()); + EXPECT_DOUBLE_EQ(0.0, orientation2.y()); + EXPECT_DOUBLE_EQ(0.0, orientation2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.z()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); + + // General 3D motion (these value are checked against rl predict() equations) + position1 = {0.0, 0.0, 0.0}; + orientation1 = {0.110, -0.003, -0.943, 0.314}; // RPY {-2.490, -0.206, 3.066} + vel_linear1 = {0.1, 0.2, 0.1}; + vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; + acc_linear1 = {-0.5, 1.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012044123300410431, position2.x()); + EXPECT_DOUBLE_EQ(0.011755776496514461, position2.y()); + EXPECT_DOUBLE_EQ(-0.024959783911094033, position2.z()); + EXPECT_DOUBLE_EQ(0.20388993714859482, orientation2.w()); + EXPECT_DOUBLE_EQ(0.061993007799788086, orientation2.x()); + EXPECT_DOUBLE_EQ(-0.90147820778463239, orientation2.y()); + EXPECT_DOUBLE_EQ(0.3767264277999153, orientation2.z()); + EXPECT_DOUBLE_EQ(0.05, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.3, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2.x()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); } TEST(Predict, predictFromDoublePointers) { double position1[3] {0.0, 0.0, 0.0}; - // double orientation1[4] {1.0, 0.0, 0.0, 0.0}; double orientation1[3] {0.0, 0.0, 0.0}; double vel_linear1[3] {1.0, 0.0, 0.0}; double vel_angular1[3] {0.0, 0.0, 1.570796327}; double acc_linear1[3] {1.0, 0.0, 0.0}; - const double dt = 0.1; - double position2[3] {1000.0, 0.0, 0.0}; + double dt = 0.1; + double position2[3]; double orientation2[3]; double vel_linear2[3]; double vel_angular2[3]; @@ -202,79 +284,164 @@ TEST(Predict, predictFromDoublePointers) EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); // Carry on with the output state from last time - show in-place update support - // fuse_models::predict( - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); - - // q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Use non-zero Y values + vel_linear1[1] = -1.0; + vel_angular1[2] = -1.570796327; + acc_linear1[1] = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); - // EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); - // EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); - // EXPECT_DOUBLE_EQ(0.0, position2[2]); - // EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); - // EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); - // EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); - // EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); - // EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); - // EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); - // EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - - // // // Use non-zero Y values - // vel_linear1[1] = -1.0; - // vel_angular1[2] = -1.570796327; - // acc_linear1[1] = -1.0; - - // fuse_models::predict( - // position1, - // orientation1, - // vel_linear1, - // vel_angular1, - // acc_linear1, - // dt, - // position2, - // orientation2, - // vel_linear2, - // vel_angular2, - // acc_linear2); + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(-0.105,position2[1]); + EXPECT_DOUBLE_EQ(-0.105, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - // q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - - // EXPECT_DOUBLE_EQ(0.105, position2[0]); - // EXPECT_DOUBLE_EQ(-0.105,position2[1]); - // EXPECT_DOUBLE_EQ(-0.105, position2[2]); - // EXPECT_DOUBLE_EQ(q.w(), orientation2[0]); - // EXPECT_DOUBLE_EQ(q.x(), orientation2[1]); - // EXPECT_DOUBLE_EQ(q.y(), orientation2[2]); - // EXPECT_DOUBLE_EQ(q.z(), orientation2[3]); - // EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - // EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); - // EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - // EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); - // EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); - // EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); - // EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); - // EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - // TODO: continue with out of plane motion + // Out of plane motion + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = 0.0; + orientation1[1] = 0.0; + orientation1[2] = 0.0; + vel_linear1[0] = 0.0; + vel_linear1[1] = 0.0; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 0.0; + vel_angular1[2] = 0.0; + acc_linear1[0] = 0.0; + acc_linear1[1] = 0.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.015, position2[2]); + EXPECT_DOUBLE_EQ(0.15707963270000003, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.0, orientation2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[2]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = -2.490; + orientation1[1] = -0.206; + orientation1[2] = 3.066; + vel_linear1[0] = 0.1; + vel_linear1[1] = 0.2; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 1.570796327; + vel_angular1[2] = -1.570796327; + acc_linear1[0] = -0.5; + acc_linear1[1] = 1.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(-0.047471798749862813, position2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); } TEST(Predict, predictFromJetPointers) @@ -286,7 +453,7 @@ TEST(Predict, predictFromJetPointers) Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; - const Jet dt = Jet(0.1); + Jet dt = Jet(0.1); Jet position2[3]; Jet orientation2[3]; Jet vel_linear2[3]; @@ -322,329 +489,163 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - // // Carry on with the output state from last time - show in-place update support -// fuse_models::predict( -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - -// EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); -// EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); -// EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); -// EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); -// EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); -// EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); -// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); -// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - -// // // Use non-zero Y values -// vel_linear1[1] = Jet(-1.0); -// vel_angular1[2] = Jet(-1.570796327); -// acc_linear1[1] = Jet(-1.0); - -// fuse_models::predict( -// position1, -// orientation1, -// vel_linear1, -// vel_angular1, -// acc_linear1, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); + // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); -// q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - -// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); -// EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[2].a); -// EXPECT_DOUBLE_EQ(q.w().a, orientation2[0].a); -// EXPECT_DOUBLE_EQ(q.x().a, orientation2[1].a); -// EXPECT_DOUBLE_EQ(q.y().a, orientation2[2].a); -// EXPECT_DOUBLE_EQ(q.z().a, orientation2[3].a); -// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); -// EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); -// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - // TODO: continue with out of plane motion -} + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); -// TEST(Predict, predictFromEigenMatrixDouble) -// { -// Eigen::Matrix position1 = {0.0, 0.0, 0.0}; -// Eigen::Quaterniond orientation1 = {1.0, 0.0, 0.0, 0.0}; -// Eigen::Matrix vel_linear1 = {1.0, 0.0, 0.0}; -// Eigen::Matrix vel_angular1 = {0.0, 0.0, 1.570796327}; -// Eigen::Matrix acc_linear1 = {1.0, 0.0, 0.0}; -// const double dt = 0.1; -// Eigen::Matrix position2; -// Eigen::Quaterniond orientation2; -// Eigen::Matrix vel_linear2; -// Eigen::Matrix vel_angular2; -// Eigen::Matrix acc_linear2; - -// fuse_models::predict_eigen( -// position1, -// orientation1, -// vel_linear1, -// vel_angular1, -// acc_linear1, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// fuse_core::Quaternion q = -// Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - -// EXPECT_DOUBLE_EQ(0.105, position2.x()); -// EXPECT_DOUBLE_EQ(0.0, position2.y()); -// EXPECT_DOUBLE_EQ(0.0, position2.z()); -// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); -// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); -// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); -// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); -// EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); -// EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); -// EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); -// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); -// EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - -// // // Carry on with the output state from last time - show in-place update support -// fuse_models::predict_eigen( -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - -// EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); -// EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); -// EXPECT_DOUBLE_EQ(0.0, position2.z()); -// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); -// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); -// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); -// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); -// EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); -// EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); -// EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); -// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); -// EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); - -// // // Use non-zero Y values -// vel_linear1[1] = -1.0; -// vel_angular1[2] = -1.570796327; -// acc_linear1[1] = -1.0; - -// fuse_models::predict_eigen( -// position1, -// orientation1, -// vel_linear1, -// vel_angular1, -// acc_linear1, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * -// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - -// EXPECT_DOUBLE_EQ(0.105, position2.x()); -// EXPECT_DOUBLE_EQ(-0.105,position2.y()); -// EXPECT_DOUBLE_EQ(0.0, position2.z()); -// EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); -// EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); -// EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); -// EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); -// EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); -// EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); -// EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); -// EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); -// EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); -// EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); -// EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); -// // TODO: continue with out of plane motion -// } - -// TEST(Predict, predictFromEigenMatrixJet) -// { -// using Jet = ceres::Jet; -// Eigen::Matrix position1 = {Jet(0.0), Jet(0.0), Jet(0.0)}; -// Eigen::Quaternion orientation1 = {Jet(1.0), Jet(0.0), Jet(0.0), Jet(0.0)}; -// Eigen::Matrix vel_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; -// Eigen::Matrix vel_angular1 = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; -// Eigen::Matrix acc_linear1 = {Jet(1.0), Jet(0.0), Jet(0.0)}; -// const Jet dt = Jet(0.1); -// Eigen::Matrix position2; -// Eigen::Quaternion orientation2; -// Eigen::Matrix vel_linear2; -// Eigen::Matrix vel_angular2; -// Eigen::Matrix acc_linear2; - -// fuse_models::predict_eigen( -// position1, -// orientation1, -// vel_linear1, -// vel_angular1, -// acc_linear1, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// Eigen::Quaternion q = -// Eigen::AngleAxis(Jet(0.1570796327), Eigen::Vector3::UnitZ()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - -// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); -// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); -// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); -// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); -// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); -// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); -// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); -// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - -// // // Carry on with the output state from last time - show in-place update support -// fuse_models::predict_eigen( -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); - -// q = Eigen::AngleAxis(Jet(0.3141592654), Eigen::Vector3::UnitZ()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - -// EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); -// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); -// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); -// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); -// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); -// EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); -// EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); -// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); - -// // // Use non-zero Y values -// vel_linear1[1] = Jet(-1.0); -// vel_angular1[2] = Jet(-1.570796327); -// acc_linear1[1] = Jet(-1.0); - -// fuse_models::predict_eigen( -// position1, -// orientation1, -// vel_linear1, -// vel_angular1, -// acc_linear1, -// dt, -// position2, -// orientation2, -// vel_linear2, -// vel_angular2, -// acc_linear2); + // // Use non-zero Y values + vel_linear1[1] = Jet(-1.0); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); -// q = Eigen::AngleAxis(Jet(-0.1570796327), Eigen::Vector3::UnitZ()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitY()) * -// Eigen::AngleAxis(Jet(0.0), Eigen::Vector3::UnitX()); - -// EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); -// EXPECT_DOUBLE_EQ(q.w().a, orientation2.w().a); -// EXPECT_DOUBLE_EQ(q.x().a, orientation2.x().a); -// EXPECT_DOUBLE_EQ(q.y().a, orientation2.y().a); -// EXPECT_DOUBLE_EQ(q.z().a, orientation2.z().a); -// EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); -// EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); -// EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); -// EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); -// EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); -// // TODO: continue with out of plane motion -// } \ No newline at end of file + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// Out of plane motion + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(0.0); + orientation1[1] = Jet(0.0); + orientation1[2] = Jet(0.0); + vel_linear1[0] = Jet(0.0); + vel_linear1[1] = Jet(0.0); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(0.0); + vel_angular1[2] = Jet(0.0); + acc_linear1[0] = Jet(0.0); + acc_linear1[1] = Jet(0.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.015).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.15707963270000003).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(-2.490); + orientation1[1] = Jet(-0.206); + orientation1[2] = Jet(3.066); + vel_linear1[0] = Jet(0.1); + vel_linear1[1] = Jet(0.2); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(1.570796327); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[0] = Jet(-0.5); + acc_linear1[1] = Jet(1.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.047471798749862813).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); +} \ No newline at end of file From e85eaa25da6a44f6e8110d47080c90cb98ba16f5 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 14 Dec 2023 11:23:46 +0000 Subject: [PATCH 041/116] feature: quat2eul + jacobian --- fuse_core/include/fuse_core/util.hpp | 80 ++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b3bdc4a55..4b4651f6d 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -150,6 +150,86 @@ Eigen::Matrix rotationMatrix2D(const T angle) return rotation; } +/** + * @brief Compute roll, pitch, and yaw from a quaternion + * + * @param[in] q Pointer to the quaternion array (4x1) + * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) + * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) + */ +static inline void quat2rpy(const double * q, double * rpy, double * jacobian = nullptr) +{ + rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); + rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); + rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); + + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + const double qw = q[0]; + const double qx = q[1]; + const double qy = q[2]; + const double qz = q[3]; + const double discr = qz * qy - qx * qz; + jacobian_map.setZero(); + + if (discr > 0.49999) { + // pitch = 90 deg + jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); + return; + } else if (discr < -0.49999) { + // pitch = -90 deg + jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); + return; + } else { + // Non-degenerate case: + jacobian_map(0, 0) = + -(2.0 * qx) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); + jacobian_map(0, 1) = + -((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 2) = + -((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 3) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); + + jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + + jacobian_map(2, 0) = + -(2.0 * qz) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 1) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 2) = + -((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + jacobian_map(2, 3) = + -((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + } + } +} + /** * @brief Create a compound ROS topic name from two components * From ba5217d8338e8ac4d9f3c509fe84291f80ad5b4e Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 14 Dec 2023 11:25:18 +0000 Subject: [PATCH 042/116] feature: unicycle 3d analytical derivatives --- .../fuse_models/unicycle_3d_predict.hpp | 303 ++++++++++++++++-- .../unicycle_3d_state_cost_function.hpp | 218 ++++++------- .../unicycle_3d_state_cost_functor.hpp | 11 +- ...unicycle_3d_state_kinematic_constraint.cpp | 14 +- .../test_unicycle_3d_state_cost_function.cpp | 111 ++++--- 5 files changed, 452 insertions(+), 205 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index de434963d..f7bcf646c 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -110,24 +111,140 @@ inline void predict( T & acc_linear2_z) { // 3D material point projection model which matches the one used by r_l. - T sr = ceres::sin(orientation1_r); - T cr = ceres::cos(orientation1_r); - T sp = ceres::sin(orientation1_p); - T cp = ceres::cos(orientation1_p); - T sy = ceres::sin(orientation1_y); - T cy = ceres::cos(orientation1_y); - T cpi = T(1.0) / cp; + const T sr = ceres::sin(orientation1_r); + const T cr = ceres::cos(orientation1_r); + const T sp = ceres::sin(orientation1_p); + const T cp = ceres::cos(orientation1_p); + const T sy = ceres::sin(orientation1_y); + const T cy = ceres::cos(orientation1_y); + const T cpi = T(1.0) / cp; + const T dt2 = T(0.5) * dt * dt; // Project the state position2_x = position1_x + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + - ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * T(0.5) * dt * dt; + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; position2_y = position1_y + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + - ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * T(0.5) * dt * dt; + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; + position2_z = position1_z + + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; + + orientation2_r = orientation1_r + + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + + vel_angular2_r = vel_angular1_r; + vel_angular2_p = vel_angular1_p; + vel_angular2_y = vel_angular1_y; + + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; + + fuse_core::wrapAngle2D(orientation2_r); + fuse_core::wrapAngle2D(orientation2_p); + fuse_core::wrapAngle2D(orientation2_y); +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1_x - First X position + * @param[in] position1_y - First Y position + * @param[in] position1_z - First Z position + * @param[in] orientation1_r - First roll orientation + * @param[in] orientation1_p - First pitch orientation + * @param[in] orientation1_y - First yaw orientation + * @param[in] vel_linear1_x - First X velocity + * @param[in] vel_linear1_y - First Y velocity + * @param[in] vel_linear1_z - First Z velocity + * @param[in] vel_angular1_r - First roll velocity + * @param[in] vel_angular1_p - First pitch velocity + * @param[in] vel_angular1_y - First yaw velocity + * @param[in] acc_linear1_x - First X acceleration + * @param[in] acc_linear1_y - First Y acceleration + * @param[in] acc_linear1_z - First Z acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2_x - Second X position + * @param[out] position2_y - Second Y position + * @param[out] position2_z - Second Z position + * @param[out] orientation2_r - Second roll orientation + * @param[out] orientation2_p - Second pitch orientation + * @param[out] orientation2_y - Second yaw orientation + * @param[out] vel_linear2_x - Second X velocity + * @param[out] vel_linear2_y - Second Y velocity + * @param[out] vel_linear2_z - Second Z velocity + * @param[out] vel_angular_r - Second roll velocity + * @param[out] vel_angular_p - Second pitch velocity + * @param[out] vel_angular_y - Second yaw velocity + * @param[out] acc_linear2_x - Second X acceleration + * @param[out] acc_linear2_y - Second Y acceleration + * @param[out] acc_linear2_z - Second Z acceleration + * @param[out] jacobians - Jacobians wrt the state + */ +inline void predict( + const double position1_x, + const double position1_y, + const double position1_z, + const double orientation1_r, + const double orientation1_p, + const double orientation1_y, + const double vel_linear1_x, + const double vel_linear1_y, + const double vel_linear1_z, + const double vel_angular1_r, + const double vel_angular1_p, + const double vel_angular1_y, + const double acc_linear1_x, + const double acc_linear1_y, + const double acc_linear1_z, + const double dt, + double & position2_x, + double & position2_y, + double & position2_z, + double & orientation2_r, + double & orientation2_p, + double & orientation2_y, + double & vel_linear2_x, + double & vel_linear2_y, + double & vel_linear2_z, + double & vel_angular2_r, + double & vel_angular2_p, + double & vel_angular2_y, + double & acc_linear2_x, + double & acc_linear2_y, + double & acc_linear2_z, + double ** jacobians, + double * jacobian_quat2rpy) +{ + // 3D material point projection model which matches the one used by r_l. + const double sr = ceres::sin(orientation1_r); + const double cr = ceres::cos(orientation1_r); + const double sp = ceres::sin(orientation1_p); + const double cp = ceres::cos(orientation1_p); + const double sy = ceres::sin(orientation1_y); + const double cy = ceres::cos(orientation1_y); + const double cpi = 1.0 / cp; + const double dt2 = 0.5 * dt * dt; + + // Project the state + position2_x = position1_x + + ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; + position2_y = position1_y + + ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + + ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; position2_z = position1_z + - ((-sp) * vel_linear1_x + (cp * sr) + vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + - ((-sp) * acc_linear1_x + (cp * sr) + acc_linear1_y + (cp * cr) * acc_linear1_z) * T(0.5) * dt * dt; + ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; orientation2_r = orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; @@ -151,10 +268,155 @@ inline void predict( fuse_core::wrapAngle2D(orientation2_r); fuse_core::wrapAngle2D(orientation2_p); fuse_core::wrapAngle2D(orientation2_y); + + // TODO(giafranchini): should we store common expressions? + if (jacobians) { + // Jacobian wrt position1 + if (jacobians[0]) { + Eigen::Map> jacobian(jacobians[0]); + jacobian.setZero(); + // partial derivatives of position2 wrt orientation1 + jacobian(0, 0) = 1.0; + jacobian(1, 1) = 1.0; + jacobian(2, 2) = 1.0; + } + // Jacobian wrt orientation1 + if (jacobians[1]) { + Eigen::Map> jacobian(jacobians[1]); + Eigen::Map> j_quat2rpy_map(jacobian_quat2rpy); + fuse_core::Matrix j_tmp; + jacobian.setZero(); + j_tmp.setZero(); + + // partial derivatives of position2_x wrt orientation1 + j_tmp(0, 0) = + ((cr * sp * cy + sr * sy) * vel_linear1_y + (- sr * sp * cy + cr * sy) * vel_linear1_z) * dt + + ((cr * sp * cy + sr * sy) * acc_linear1_y + (- sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; + j_tmp(0, 1) = + ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + + ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; + j_tmp(0, 2) = + ((-cp * sy) * vel_linear1_x + (- sr * sp * sy - cr * cy) * vel_linear1_y + (- cr * sp * sy + sr * cy) * vel_linear1_z) * dt + + ((-cp * sy) * acc_linear1_x + (- sr * sp * sy - cr * cy) * acc_linear1_y + (- cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; + + // partial derivatives of position2_y wrt orientation1 + j_tmp(1, 0) = + ((- sr * cy + cr * sp * sy) * vel_linear1_y + (- cr * cy - sr * sp * sy) * vel_linear1_z) * dt + + ((- sr * cy + cr * sp * sy) * acc_linear1_y + (- cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 1) = + ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + + ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 2) = + ((cp * cy) * vel_linear1_x + (- cr * sy + sr * sp * cy) * vel_linear1_y + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (- cr * sy + sr * sp * cy) * acc_linear1_y + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; + + // partial derivatives of position2_z wrt orientation1 + j_tmp(2, 0) = + ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + + ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; + j_tmp(2, 1) = + (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + + (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; + + // partial derivatives of orientation2_r wrt orientation1 + j_tmp(3, 0) = 1.0 + + ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; + j_tmp(3, 1) = + ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr *sp * sp * cpi * cpi) * vel_angular1_y) * dt; + + // partial derivatives of orientation2_p wrt orientation1 + j_tmp(4, 0) = (-sr * vel_angular1_p - cr * vel_angular1_y) * dt; + j_tmp(4, 1) = 1.0; + + // partial derivatives of orientation2_y wrt orientation1 + j_tmp(5, 0) = ((cr * cpi) * vel_angular1_p - (sr * cpi) * vel_angular1_y) * dt; + j_tmp(5, 1) = ((sp * sr * cpi * cpi) * vel_angular1_p + (cr * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp(5, 2) = 1.0; + + jacobian.block<6, 4>(0, 0) = j_tmp * j_quat2rpy_map; + } + // Jacobian wrt vel_linear1 + if (jacobians[2]) { + Eigen::Map> jacobian(jacobians[2]); + jacobian.setZero(); + + // partial derivatives of position1_x wrt vel_linear1 + jacobian(0, 0) = cp * cy * dt; + jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; + // partial derivatives of position1_y wrt vel_linear1 + jacobian(1, 0) = cp * sy * dt; + jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt; + jacobian(1, 2) = (cr * sp * sy - sr * cy) * dt; + // partial derivatives of position1_z wrt vel_linear1 + jacobian(2, 0) = -sp * dt; + jacobian(2, 1) = sr * cp * dt; + jacobian(2, 2) = cr * cp * dt; + // partial derivatives of vel_linear2_x wrt vel_linear1 + jacobian(6, 0) = 1.0; + // partial derivatives of vel_linear2_y wrt vel_linear1 + jacobian(7, 1) = 1.0; + // partial derivatives of vel_linear2_z wrt vel_linear1 + jacobian(8, 2) = 1.0; + } + + // Jacobian wrt vel_angular1 + if (jacobians[3]) { + Eigen::Map> jacobian(jacobians[3]); + jacobian.setZero(); + + // partial derivatives of orientation2_r wrt vel_angular1 + jacobian(3, 0) = dt; + jacobian(3, 1) = sr * sp * cpi * dt; + jacobian(3, 2) = cr * sp * cpi * dt; + // partial derivatives of orientation2_p wrt vel_angular1 + jacobian(4, 1) = cr * dt; + jacobian(4, 2) = -sr * dt; + // partial derivatives of orientation2_y wrt vel_angular1 + jacobian(5, 1) = sr * cpi * dt; + jacobian(5, 2) = cr * cpi * dt; + // partial derivatives of vel_angular2_r wrt vel_angular1 + jacobian(9, 0) = 1.0; + // partial derivatives of vel_angular2_p wrt vel_angular1 + jacobian(10, 1) = 1.0; + // partial derivatives of vel_angular2_y wrt vel_angular1 + jacobian(11, 2) = 1.0; + } + + // Jacobian wrt acc_linear1 + if (jacobians[4]) { + Eigen::Map> jacobian(jacobians[4]); + jacobian.setZero(); + // partial derivatives of position1_x wrt acc_linear1 + jacobian(0, 0) = cp * cy * dt2; + jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt2; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; + // partial derivatives of position1_y wrt acc_linear1 + jacobian(1, 0) = cp * sy * dt2; + jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt2; + jacobian(1, 2) = (cr * sp * sy - sr * cy) * dt2; + // partial derivatives of position1_z wrt acc_linear1 + jacobian(2, 0) = -sp * dt2; + jacobian(2, 1) = sr * cp * dt2; + jacobian(2, 2) = cr * cp * dt2; + // partial derivatives of vel_linear2_x wrt acc_linear1 + jacobian(6, 0) = dt; + // partial derivatives of vel_linear2_y wrt acc_linear1 + jacobian(7, 1) = dt; + // partial derivatives of vel_linear2_z wrt acc_linear1 + jacobian(8, 2) = dt; + // partial derivatives of acc_linear2_x wrt acc_linear1 + jacobian(12, 0) = 1.0; + // partial derivatives of acc_linear2_y wrt acc_linear1 + jacobian(13, 1) = 1.0; + // partial derivatives of acc_linear2_z wrt acc_linear1 + jacobian(14, 2) = 1.0; + } } +} /** - * @brief Given a state and time delta, predicts a new state - + * @brief Given a state and time delta, predicts a new state * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) * @param[in] orientation1 - First orientation (array with roll at index 0, pitch at index 1, yaw at index 2) * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) @@ -248,13 +510,13 @@ inline void predict( fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) ); // 3D material point projection model which matches the one used by r_l. - double sr = ceres::sin(rpy.x()); - double cr = ceres::cos(rpy.x()); - double sp = ceres::sin(rpy.y()); - double cp = ceres::cos(rpy.y()); - double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - double cy = ceres::cos(rpy.z()); - double cpi = 1.0 / cp; + const double sr = ceres::sin(rpy.x()); + const double cr = ceres::cos(rpy.x()); + const double sp = ceres::sin(rpy.y()); + const double cp = ceres::cos(rpy.y()); + const double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model + const double cy = ceres::cos(rpy.z()); + const double cpi = 1.0 / cp; fuse_core::Matrix3d tf_pos, tf_rot; tf_pos(0, 0) = (cy * cp); @@ -292,7 +554,8 @@ inline void predict( orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - } +} + } // namespace fuse_models #endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp index d999f7f97..d096fc027 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp @@ -50,43 +50,31 @@ namespace fuse_models * @brief Create a cost function for a 3D state vector * * The state vector includes the following quantities, given in this order: - * x position - * y position - * z position - * roll (rotation about the x axis) - * pitch (rotation about the y axis) - * yaw (rotation about the z axis) - * x velocity - * y velocity - * z velocity - * roll velocity - * pitch velocity - * yaw velocity - * x acceleration - * y acceleration - * z acceleration + * position (x, y, z) + * orientation (roll, pitch, yaw) + * linear velocity (x, y, z) + * angular velocity (roll, pitch, yaw) + * linear acceleration (x, y, z) * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost * function that applies a prior constraint on both the entire state vector. * * The cost function is of the form: * - * || [ x_t2 - proj(x_t1) ] ||^2 - * cost(x) = || [ y_t2 - proj(y_t1) ] || - * || [ z_t2 - proj(z_t1) ] || - * || [ roll_t2 - proj(roll_t1) ] || - * || [ pitch_t2 - proj(pitch_t1) ] || - * || [ yaw_t2 - proj(yaw_t1) ] || - * || A * [ x_vel_t2 - proj(x_vel_t1) ] || - * || [ y_vel_t2 - proj(y_vel_t1) ] || - * || [ z_vel_t2 - proj(z_vel_t1) ] || - * || [ roll_vel_t2 - proj(roll_vel_t1) ] || - * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || - * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || - * || [ x_acc_t2 - proj(x_acc_t1) ] || - * || [ y_acc_t2 - proj(y_acc_t1) ] || - * || [ z_acc_t2 - proj(z_acc_t1) ] || - * + * || [ x_t2 - proj(x_t1) ] ||^2 + * || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ quat2eul(rpy_t2) - proj(quat2eul(rpy_t1)) ] || + * cost(x) = || A *[ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ z_vel_t2 - proj(z_vel_t1) ] || + * || [ roll_vel_t2 - proj(roll_vel_t1) ] || + * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ z_acc_t2 - proj(z_acc_t1) ] || + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and * proj is a function that projects the state variables from time t1 to time t2. In case the user is * interested in implementing a cost function of the form @@ -115,21 +103,27 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, * @brief Evaluate the cost function. Used by the Ceres optimization engine. * * @param[in] parameters - Parameter blocks: - * 0 : position1 - First position (array with x at index 0, y at index 1, z at index 2) - * 1 : orientation1 - First orientation (array with roll at index 0, pitch at index 1, yaw at index 2) - * 2 : vel_linear1 - First linear velocity (array with vx at index 0, vy at index 1, - * vz at index 2) - * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) - * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, + * 0 : position1 - First position (array with x at index 0, y at index 1, * z at index 2) - * 5 : position2 - Second position (array with x at index 0, y at index 1, z at index 2) - * 6 : orientation2 - Second orientation (array with roll at index 0, pitch at index 1, yaw at index 2) - * 7 : vel_linear2 - Second linear velocity (array with vx at index 0, vy at index 1, - * vz at index 2) - * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) - * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, + * 1 : orientation1 - First orientation (array with w at index 0, x at index 1, + * y at index 2, z at index 3) + * 2 : vel_linear1 - First linear velocity (array with x at index 0, y at + * index 1, z at index 2) + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, + * vpitch at index 1, vyaw at index 2) + * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y + * at index 1, z at index 2) + * 5 : position2 - Second position (array with x at index 0, y at index 1, * z at index 2) - * @param[out] residual - The computed residual (error) + * 6 : orientation2 - Second orientation (array with w at index 0, x at index 1, + * y at index 2, z at index 3) + * 7 : vel_linear2 - Second linear velocity (array with x at index 0, y at + * index 1, z at index 2) + * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, + * vpitch at index 1, vyaw at index 2) + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at + * index 1) + * @param[out] residual - The computed residual (error) * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not * NULL, and only computed for the parameters where jacobians[i] is not * NULL. @@ -141,41 +135,69 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, double * residuals, double ** jacobians) const override { - double position_pred_x, position_pred_y, position_pred_z; - double roll_pred, pitch_pred, yaw_pred; - double vel_linear_pred_x, vel_linear_pred_y, vel_linear_pred_z; - double vel_roll_pred, vel_pitch_pred, vel_yaw_pred; - double acc_linear_pred_x, acc_linear_pred_y, acc_linear_pred_z; + double position_pred[3]; + double orientation_pred_rpy[3]; + double vel_linear_pred[3]; + double vel_angular_pred[3]; + double acc_linear_pred[3]; + + double orientation1_rpy[3]; + double orientation2_rpy[3]; + double j1_quat2rpy[12]; + double j2_quat2rpy[12]; + fuse_core::quat2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); + fuse_core::quat2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); predict( - parameters[0][0], parameters[0][2], parameters[0][2], - parameters[1][0], parameters[1][2], parameters[1][2], - parameters[2][0], parameters[2][2], parameters[2][2], - parameters[3][0], parameters[3][2], parameters[3][2], - parameters[4][0], parameters[4][2], parameters[4][2], + parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[0][2], // position1_z + orientation1_rpy[0], // orientation1_roll + orientation1_rpy[1], // orientation1_pitch + orientation1_rpy[2], // orientation1_yaw + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[2][2], // vel_linear1_z + parameters[3][0], // vel_angular1_roll + parameters[3][1], // vel_angular1_pitch + parameters[3][2], // vel_angular1_yaw + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + parameters[4][2], // acc_linear1_z dt_, - position_pred_x, position_pred_y, position_pred_z, - roll_pred, pitch_pred, yaw_pred, - vel_linear_pred_x, vel_linear_pred_y, vel_linear_pred_z, - vel_roll_pred, vel_pitch_pred, vel_yaw_pred, - acc_linear_pred_x, acc_linear_pred_y, acc_linear_pred_z, - jacobians); + position_pred[0], + position_pred[1], + position_pred[2], + orientation_pred_rpy[0], + orientation_pred_rpy[1], + orientation_pred_rpy[2], + vel_linear_pred[0], + vel_linear_pred[1], + vel_linear_pred[2], + vel_angular_pred[0], + vel_angular_pred[1], + vel_angular_pred[2], + acc_linear_pred[0], + acc_linear_pred[1], + acc_linear_pred[2], + jacobians, + j1_quat2rpy); - residuals[0] = parameters[5][0] - position_pred_x; - residuals[1] = parameters[5][1] - position_pred_y; - residuals[2] = parameters[5][2] - position_pred_z; - residuals[3] = parameters[6][0] - roll_pred; - residuals[4] = parameters[6][1] - pitch_pred; - residuals[5] = parameters[6][2] - yaw_pred; - residuals[6] = parameters[7][0] - vel_linear_pred_x; - residuals[7] = parameters[7][1] - vel_linear_pred_y; - residuals[8] = parameters[7][2] - vel_linear_pred_z; - residuals[9] = parameters[8][0] - vel_roll_pred; - residuals[10] = parameters[8][1] - vel_pitch_pred; - residuals[11] = parameters[8][2] - vel_yaw_pred; - residuals[12] = parameters[9][0] - acc_linear_pred_x; - residuals[13] = parameters[9][1] - acc_linear_pred_y; - residuals[14] = parameters[9][2] - acc_linear_pred_z; + residuals[0] = parameters[5][0] - position_pred[0]; + residuals[1] = parameters[5][1] - position_pred[1]; + residuals[2] = parameters[5][2] - position_pred[2]; + residuals[3] = orientation2_rpy[0] - orientation_pred_rpy[0]; + residuals[4] = orientation2_rpy[1] - orientation_pred_rpy[1]; + residuals[5] = orientation2_rpy[2] - orientation_pred_rpy[2]; + residuals[6] = parameters[7][0] - vel_linear_pred[0]; + residuals[7] = parameters[7][1] - vel_linear_pred[1]; + residuals[8] = parameters[7][2] - vel_linear_pred[2]; + residuals[9] = parameters[8][0] - vel_angular_pred[0]; + residuals[10] = parameters[8][1] - vel_angular_pred[1]; + residuals[11] = parameters[8][2] - vel_angular_pred[2]; + residuals[12] = parameters[9][0] - acc_linear_pred[0]; + residuals[13] = parameters[9][1] - acc_linear_pred[1]; + residuals[14] = parameters[9][2] - acc_linear_pred[2]; fuse_core::wrapAngle2D(residuals[3]); fuse_core::wrapAngle2D(residuals[4]); @@ -187,29 +209,15 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, residuals_map.applyOnTheLeft(A_); if (jacobians) { - // It might be possible to simplify the code below implementing something like this but using - // compile-time template recursion. - // - // // state1: (position1, yaw1, vel_linear1, vel_yaw1, acc_linear1) - // for (size_t i = 0; i < 5; ++i) - // { - // if (jacobians[i]) - // { - // Eigen::Map> jacobian( - // jacobians[i]); - // jacobian.applyOnTheLeft(-A_); - // } - // } - // Update jacobian wrt position1 if (jacobians[0]) { Eigen::Map> jacobian(jacobians[0]); jacobian.applyOnTheLeft(-A_); } - // Update jacobian wrt orientation + // Update jacobian wrt orientation1 if (jacobians[1]) { - Eigen::Map> jacobian(jacobians[1]); + Eigen::Map> jacobian(jacobians[1]); jacobian.applyOnTheLeft(-A_); } @@ -219,7 +227,7 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, jacobian.applyOnTheLeft(-A_); } - // Update jacobian wrt vel_angular1 + // Update jacobian wrt vel_yaw1 if (jacobians[3]) { Eigen::Map> jacobian(jacobians[3]); jacobian.applyOnTheLeft(-A_); @@ -230,24 +238,7 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } - - // It might be possible to simplify the code below implementing something like this but using - // compile-time template recursion. - // - // // state2: (position2, yaw2, vel_linear2, vel_yaw2, acc_linear2) - // for (size_t i = 5, offset = 0; i < ParameterDims::kNumParameterBlocks; ++i) - // { - // constexpr auto dim = ParameterDims::GetDim(i); - - // if (jacobians[i]) - // { - // Eigen::Map> jacobian(jacobians[i]); - // jacobian = A_.block<8, dim>(0, offset); - // } - - // offset += dim; - // } - + // Jacobian wrt position2 if (jacobians[5]) { Eigen::Map> jacobian(jacobians[5]); @@ -256,8 +247,9 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, // Jacobian wrt orientation2 if (jacobians[6]) { - Eigen::Map> jacobian(jacobians[6]); - jacobian = A_.block<15, 3>(0, 3); + Eigen::Map> jacobian(jacobians[6]); + Eigen::Map> j2_quat2rpy_map(j2_quat2rpy); + jacobian = A_.block<15, 3>(0, 3) * j2_quat2rpy_map; } // Jacobian wrt vel_linear2 @@ -285,7 +277,7 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, private: double dt_; fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix + //!< information matrix }; Unicycle3DStateCostFunction::Unicycle3DStateCostFunction( diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index a822c7ba7..f96676936 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -147,12 +147,11 @@ bool Unicycle3DStateCostFunctor::operator()( const T * const acc_linear2, T * residual) const { - // Initialize predicted state variables - T position_pred[3] {T(0.0)}; - T orientation_pred[3] {T(0.0)}; - T vel_linear_pred[3] {T(0.0)}; - T vel_angular_pred[3] {T(0.0)}; - T acc_linear_pred[3] {T(0.0)}; + T position_pred[3]; + T orientation_pred[3]; + T vel_linear_pred[3]; + T vel_angular_pred[3]; + T acc_linear_pred[3]; // Convert orientation variables from quaternion to roll-pitch-yaw const T orientation1_rpy[3] { diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp index 675240059..0f88ad6c6 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -37,8 +37,8 @@ #include #include -// #include -#include +#include +// #include #include #include #include @@ -101,16 +101,12 @@ void Unicycle3DStateKinematicConstraint::print(std::ostream & stream) const ceres::CostFunction * Unicycle3DStateKinematicConstraint::costFunction() const { + return new Unicycle3DStateCostFunction(dt_, sqrt_information_); // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // - return new ceres::AutoDiffCostFunction( - new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); - // - // which requires: - // - // #include - // return new Unicycle3DStateCostFunction(dt_, sqrt_information_); + // return new ceres::AutoDiffCostFunction( + // new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); } } // namespace fuse_models diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp index 4ace46cea..c0ce74508 100644 --- a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -41,7 +41,7 @@ #include #include -// #include +#include #include TEST(CostFunction, evaluateCostFunction) @@ -55,25 +55,25 @@ TEST(CostFunction, evaluateCostFunction) const double dt{0.1}; const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; - // const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; // Evaluate cost function - const double position1[] = {0.0, 0.0, 0.0}; - const double orientation1[] = {1.0, 0.0, 0.0, 0.0}; - const double vel_linear1[] = {1.0, 0.0, 0.0}; - const double vel_angular1[] = {0.0, 0.0, 1.570796327}; - const double acc_linear1[] = {1.0, 0.0, 0.0}; + const double position1[3] = {0.0, 0.0, 0.0}; + const double orientation1[4] = {1.0, 0.0, 0.0, 0.0}; + const double vel_linear1[3] = {1.0, 1.0, 1.0}; + const double vel_angular1[3] = {1.570796327, 1.570796327, 1.570796327}; + const double acc_linear1[3] = {1.0, 1.0, 1.0}; - const double position2[] = {0.105, 0.0, 0.0}; + const double position2[3] = {0.105, 0.105, 0.105}; Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - const double orientation2[] = {q2.w(), q2.x(), q2.y(), q2.z()}; - const double vel_linear2[] = {1.1, 0.0, 0.0}; - const double vel_angular2[] = {0.0, 0.0, 1.570796327}; - const double acc_linear2[] = {1.0, 0.0, 0.0}; - - const double * parameters[] = + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); + const double orientation2[4] = {q2.w(), q2.x(), q2.y(), q2.z()}; + const double vel_linear2[3] = {1.1, 1.1, 1.1}; + const double vel_angular2[3] = {1.570796327, 1.570796327, 1.570796327}; + const double acc_linear2[3] = {1.0, 1.0, 1.0}; + + const double * parameters[10] = { position1, orientation1, vel_linear1, vel_angular1, acc_linear1, position2, orientation2, vel_linear2, vel_angular2, acc_linear2 @@ -81,50 +81,47 @@ TEST(CostFunction, evaluateCostFunction) fuse_core::Vector15d residuals; -// const auto & block_sizes = cost_function.parameter_block_sizes(); -// const auto num_parameter_blocks = block_sizes.size(); + const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto num_parameter_blocks = block_sizes.size(); -// const auto num_residuals = cost_function.num_residuals(); + const auto num_residuals = cost_function.num_residuals(); -// std::vector J(num_parameter_blocks); -// std::vector jacobians(num_parameter_blocks); + std::vector J(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); -// for (size_t i = 0; i < num_parameter_blocks; ++i) { -// J[i].resize(num_residuals, block_sizes[i]); -// jacobians[i] = J[i].data(); -// } + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } -// EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); + EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); -// // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 -// // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 -// EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); + // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 + // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 + EXPECT_MATRIX_NEAR(fuse_core::Vector15d::Zero(), residuals, 1e-15); // // Check jacobians are correct using a gradient checker -// ceres::NumericDiffOptions numeric_diff_options; - -// #if CERES_VERSION_AT_LEAST(2, 1, 0) -// std::vector parameterizations; -// ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); -// #else -// ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); -// #endif - -// // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error -// // is 5.26356e-10 -// ceres::GradientChecker::ProbeResults probe_results; -// // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, -// // but Probe actually returns true and the jacobians are correct according to the -// // gradient checker numeric differentiation -// // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << -// // probe_results.error_log; + ceres::NumericDiffOptions numeric_diff_options; + +#if CERES_VERSION_AT_LEAST(2, 1, 0) + std::vector parameterizations; + ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); +#else + ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +#endif + + // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error + // is 5.26356e-10 + ceres::GradientChecker::ProbeResults probe_results; + // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, + // but Probe actually returns true and the jacobians are correct according to the + // gradient checker numeric differentiation + // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << + // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor ceres::AutoDiffCostFunction - cost_function_autodiff(new fuse_models::Unicycle3DStateCostFunctor(dt, sqrt_information)); - auto num_parameter_blocks = cost_function_autodiff.parameter_block_sizes().size(); - auto num_residuals = cost_function_autodiff.num_residuals(); + 3>cost_function_autodiff(new fuse_models::Unicycle3DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); std::vector jacobians_autodiff(num_parameter_blocks); @@ -139,12 +136,12 @@ TEST(CostFunction, evaluateCostFunction) parameters, residuals.data(), jacobians_autodiff.data())); - // const Eigen::IOFormat HeavyFmt( - // Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - // for (size_t i = 0; i < num_parameter_blocks; ++i) { - // EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) - // << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - // << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); - // } + for (size_t i = 0; i < num_parameter_blocks; ++i) { + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-4) + << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) + << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + } } From 4f7dff89b6468eca45b3e4a8bfe4ccb368426a63 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 14 Dec 2023 16:58:59 +0000 Subject: [PATCH 043/116] refactor: jacobian computation in predict function with eigen --- .../fuse_models/unicycle_3d_predict.hpp | 168 +++++++++++++----- fuse_models/test/test_unicycle_3d_predict.cpp | 8 +- 2 files changed, 132 insertions(+), 44 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index f7bcf646c..71ffc37bd 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -503,52 +503,44 @@ inline void predict( fuse_core::Vector3d & vel_angular2, fuse_core::Vector3d & acc_linear2) { - // Convert quaternion to eigen fuse_core::Vector3d rpy( fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) ); - // 3D material point projection model which matches the one used by r_l. - const double sr = ceres::sin(rpy.x()); - const double cr = ceres::cos(rpy.x()); - const double sp = ceres::sin(rpy.y()); - const double cp = ceres::cos(rpy.y()); - const double sy = ceres::sin(rpy.z()); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model - const double cy = ceres::cos(rpy.z()); - const double cpi = 1.0 / cp; - - fuse_core::Matrix3d tf_pos, tf_rot; - tf_pos(0, 0) = (cy * cp); - tf_pos(0, 1) = (cy * sp * sr - sy * cr); - tf_pos(0, 2) = (cy * sp * cr + sy * sr); - tf_pos(1, 0) = (sy * cp); - tf_pos(1, 1) = (sy * sp * sr + cy * cr); - tf_pos(1, 2) = (sy * sp * cr - cy * sr); - tf_pos(2, 0) = (-sp); - tf_pos(2, 1) = (cp * sr); - tf_pos(2, 2) = (cp * cr); - - tf_rot(0, 0) = 1.0; - tf_rot(0, 1) = sr * sp * cpi; - tf_rot(0, 2) = cr * sp * cpi; - tf_rot(1, 0) = 0.0; - tf_rot(1, 1) = cr; - tf_rot(1, 2) = -sr; - tf_rot(2, 0) = 0.0; - tf_rot(2, 1) = sr * cpi; - tf_rot(2, 2) = cr * cpi; - // Project the state - position2 = position1 + tf_pos * vel_linear1 * dt + tf_pos * acc_linear1 * 0.5 * dt * dt; - rpy = rpy + tf_rot * vel_angular1 * dt; - vel_linear2 = vel_linear1 + acc_linear1 * dt; - vel_angular2 = vel_angular1; - acc_linear2 = acc_linear1; - - fuse_core::wrapAngle2D(rpy.x()); - fuse_core::wrapAngle2D(rpy.y()); - fuse_core::wrapAngle2D(rpy.z()); + predict( + position1.x(), + position1.y(), + position1.z(), + rpy.x(), + rpy.y(), + rpy.z(), + vel_linear1.x(), + vel_linear1.y(), + vel_linear1.z(), + vel_angular1.x(), + vel_angular1.y(), + vel_angular1.z(), + acc_linear1.x(), + acc_linear1.y(), + acc_linear1.z(), + dt, + position2.x(), + position2.y(), + position2.z(), + rpy.x(), + rpy.y(), + rpy.z(), + vel_linear2.x(), + vel_linear2.y(), + vel_linear2.z(), + vel_angular2.x(), + vel_angular2.y(), + vel_angular2.z(), + acc_linear2.x(), + acc_linear2.y(), + acc_linear2.z()); // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * @@ -556,6 +548,102 @@ inline void predict( Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); } +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation (quaternion) + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation (quaternion) + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration + * @param[out] jacobian - Jacobian wrt the state + */ +inline void predict( + const fuse_core::Vector3d & position1, + const fuse_core::Quaternion & orientation1, + const fuse_core::Vector3d & vel_linear1, + const fuse_core::Vector3d & vel_angular1, + const fuse_core::Vector3d & acc_linear1, + const double dt, + fuse_core::Vector3d & position2, + fuse_core::Quaternion & orientation2, + fuse_core::Vector3d & vel_linear2, + fuse_core::Vector3d & vel_angular2, + fuse_core::Vector3d & acc_linear2, + fuse_core::Matrix15d & jacobian) +{ + + double quat[4] = {orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()}; + double rpy[3]; + double jacobian_quat2rpy[12]; + fuse_core::quat2rpy(quat, rpy, jacobian_quat2rpy); + + // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each + // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per + // parameter block and later reconstruct the fuse_core::Matrix15d with the full jacobian. The + // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, + // vel_angular1: 3, acc_linear1: 3} + + static constexpr size_t num_residuals{15}; + static constexpr size_t num_parameter_blocks{5}; + static const std::array block_sizes = {3, 4, 3, 3, 3}; + + std::array J; + std::array jacobians; + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } + + predict( + position1.x(), + position1.y(), + position1.z(), + rpy[0], + rpy[1], + rpy[2], + vel_linear1.x(), + vel_linear1.y(), + vel_linear1.z(), + vel_angular1.x(), + vel_angular1.y(), + vel_angular1.z(), + acc_linear1.x(), + acc_linear1.y(), + acc_linear1.z(), + dt, + position2.x(), + position2.y(), + position2.z(), + rpy[0], + rpy[1], + rpy[2], + vel_linear2.x(), + vel_linear2.y(), + vel_linear2.z(), + vel_angular2.x(), + vel_angular2.y(), + vel_angular2.z(), + acc_linear2.x(), + acc_linear2.y(), + acc_linear2.z(), + jacobians.data(), + jacobian_quat2rpy); + + jacobian << J[0], J[1], J[2], J[3], J[4]; + + // Convert back to quaternion + orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); +} + } // namespace fuse_models #endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index a247296ce..314f4fbc2 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -333,7 +333,7 @@ TEST(Predict, predictFromDoublePointers) EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(-0.105,position2[1]); - EXPECT_DOUBLE_EQ(-0.105, position2[2]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); @@ -429,7 +429,7 @@ TEST(Predict, predictFromDoublePointers) EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); - EXPECT_DOUBLE_EQ(-0.047471798749862813, position2[2]); + EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); @@ -539,7 +539,7 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); @@ -635,7 +635,7 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(-0.047471798749862813).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); From 3959d7647aabd4656ce070938f282efba2b6b62d Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 14 Dec 2023 16:59:46 +0000 Subject: [PATCH 044/116] feature: odometry_3d_publisher predict to current time --- .../odometry_3d_publisher_params.hpp | 16 +- fuse_models/src/odometry_3d_publisher.cpp | 219 ++++++++---------- 2 files changed, 110 insertions(+), 125 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 64dc5b485..f4acf4de8 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -117,13 +117,18 @@ struct Odometry3DPublisherParams : public ParameterBase ns, "scale_process_noise"), scale_process_noise); - velocity_norm_min = + velocity_linear_norm_min_ = fuse_core::getParam( interfaces, fuse_core::joinParameterName( ns, - "velocity_norm_min"), - velocity_norm_min); - + "velocity_linear_norm_min"), + velocity_linear_norm_min_); + velocity_angular_norm_min_ = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_angular_norm_min"), + velocity_angular_norm_min_); fuse_core::getPositiveParam( interfaces, fuse_core::joinParameterName( @@ -220,7 +225,8 @@ struct Odometry3DPublisherParams : public ParameterBase double publish_frequency {10.0}; fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix bool scale_process_noise{false}; - double velocity_norm_min{1e-3}; + double velocity_linear_norm_min_{1e-3}; + double velocity_angular_norm_min_{1e-3}; rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds //!< to compute the covariance rclcpp::Duration tf_cache_time {10, 0}; diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 89fff0a18..179159884 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -472,126 +472,105 @@ void Odometry3DPublisher::publishTimerCallback() // If requested, we need to project our state forward in time using the 3D kinematic model if (params_.predict_to_current_time) { - // TODO: this is in pause since we need to implement the jacobian of the 3D predict function - // rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); - // fuse_core::Vector3d velocity_linear, velocity_angular; - // tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); - // tf2::fromMsg(odom_output.twist.twist.angular, velocity_angular); - - // const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); - - // fuse_core::Matrix8d jacobian; // TODO: maybe this is not useful if we rely on covariance_geometry - - // tf2::Vector3 acceleration_linear; - // if (params_.predict_with_acceleration) { - // tf2::fromMsg(acceleration_output.accel.accel.linear, acceleration_linear); - // } - - // double yaw_vel; - - // predict( - // pose, - // velocity_linear, - // velocity_angular, - // acceleration_linear, - // dt, - // pose, - // velocity_linear, - // velocity_angular, - // acceleration_linear); - - // odom_output.pose.pose.position = tf2::toMsg(pose.getOrigin()); - // odom_output.pose.pose.orientation = tf2::toMsg(pose.getRotation()); - - // odom_output.twist.twist.linear.x = velocity_linear.x(); - // odom_output.twist.twist.linear.y = velocity_linear.y(); - // odom_output.twist.twist.linear.z = velocity_linear.z(); - // odom_output.twist.twist.angular.x = velocity_angular.x(); - // odom_output.twist.twist.angular.y = velocity_angular.y(); - // odom_output.twist.twist.angular.z = velocity_angular.z(); - - // if (params_.predict_with_acceleration) { - // acceleration_output.accel.accel.linear.x = acceleration_linear.x(); - // acceleration_output.accel.accel.linear.y = acceleration_linear.y(); - // acceleration_output.accel.accel.linear.z = acceleration_linear.z(); - // } - - // odom_output.header.stamp = timer_now; - // acceleration_output.header.stamp = timer_now; - - // // Either the last covariance computation was skipped because there was no subscriber, - // // or it failed - // if (latest_covariance_valid) { - // fuse_core::Matrix8d covariance; - // covariance(0, 0) = odom_output.pose.covariance[0]; - // covariance(0, 1) = odom_output.pose.covariance[1]; - // covariance(0, 2) = odom_output.pose.covariance[5]; - // covariance(1, 0) = odom_output.pose.covariance[6]; - // covariance(1, 1) = odom_output.pose.covariance[7]; - // covariance(1, 2) = odom_output.pose.covariance[11]; - // covariance(2, 0) = odom_output.pose.covariance[30]; - // covariance(2, 1) = odom_output.pose.covariance[31]; - // covariance(2, 2) = odom_output.pose.covariance[35]; - - // covariance(3, 3) = odom_output.twist.covariance[0]; - // covariance(3, 4) = odom_output.twist.covariance[1]; - // covariance(3, 5) = odom_output.twist.covariance[5]; - // covariance(4, 3) = odom_output.twist.covariance[6]; - // covariance(4, 4) = odom_output.twist.covariance[7]; - // covariance(4, 5) = odom_output.twist.covariance[11]; - // covariance(5, 3) = odom_output.twist.covariance[30]; - // covariance(5, 4) = odom_output.twist.covariance[31]; - // covariance(5, 5) = odom_output.twist.covariance[35]; - - // covariance(6, 6) = acceleration_output.accel.covariance[0]; - // covariance(6, 7) = acceleration_output.accel.covariance[1]; - // covariance(7, 6) = acceleration_output.accel.covariance[6]; - // covariance(7, 7) = acceleration_output.accel.covariance[7]; - - // // TODO(efernandez) for now we set to zero the out-of-diagonal blocks with the correlations - // // between pose, twist and acceleration, but we could cache them in another - // // attribute when we retrieve the covariance from the ceres problem - // covariance.topRightCorner<3, 5>().setZero(); - // covariance.bottomLeftCorner<5, 3>().setZero(); - // covariance.block<3, 2>(3, 6).setZero(); - // covariance.block<2, 3>(6, 3).setZero(); - - // covariance = jacobian * covariance * jacobian.transpose(); - - // auto process_noise_covariance = params_.process_noise_covariance; - // if (params_.scale_process_noise) { - // common::scaleProcessNoiseCovariance( - // process_noise_covariance, velocity_linear, - // odom_output.twist.twist.angular.z, params_.velocity_norm_min); - // } - - // covariance.noalias() += dt * process_noise_covariance; - - // odom_output.pose.covariance[0] = covariance(0, 0); - // odom_output.pose.covariance[1] = covariance(0, 1); - // odom_output.pose.covariance[5] = covariance(0, 2); - // odom_output.pose.covariance[6] = covariance(1, 0); - // odom_output.pose.covariance[7] = covariance(1, 1); - // odom_output.pose.covariance[11] = covariance(1, 2); - // odom_output.pose.covariance[30] = covariance(2, 0); - // odom_output.pose.covariance[31] = covariance(2, 1); - // odom_output.pose.covariance[35] = covariance(2, 2); - - // odom_output.twist.covariance[0] = covariance(3, 3); - // odom_output.twist.covariance[1] = covariance(3, 4); - // odom_output.twist.covariance[5] = covariance(3, 5); - // odom_output.twist.covariance[6] = covariance(4, 3); - // odom_output.twist.covariance[7] = covariance(4, 4); - // odom_output.twist.covariance[11] = covariance(4, 5); - // odom_output.twist.covariance[30] = covariance(5, 3); - // odom_output.twist.covariance[31] = covariance(5, 4); - // odom_output.twist.covariance[35] = covariance(5, 5); - - // acceleration_output.accel.covariance[0] = covariance(6, 6); - // acceleration_output.accel.covariance[1] = covariance(6, 7); - // acceleration_output.accel.covariance[6] = covariance(7, 6); - // acceleration_output.accel.covariance[7] = covariance(7, 7); - // } + rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); + + // Convert pose in Eigen representation + fuse_core::Vector3d position, velocity_linear, velocity_angular; + fuse_core::Quaternion orientation; + position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); + orientation.x() = pose.getRotation().x(); + orientation.y() = pose.getRotation().y(); + orientation.z() = pose.getRotation().z(); + orientation.w() = pose.getRotation().w(); + velocity_linear << odom_output.twist.twist.linear.x, + odom_output.twist.twist.linear.y, odom_output.twist.twist.linear.z; + velocity_angular << odom_output.twist.twist.angular.x, + odom_output.twist.twist.angular.y, odom_output.twist.twist.angular.z; + + const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + + fuse_core::Matrix15d jacobian; + + fuse_core::Vector3d acceleration_linear; + if (params_.predict_with_acceleration) { + acceleration_linear << acceleration_output.accel.accel.linear.x, + acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; + } + + predict( + position, + orientation, + velocity_linear, + velocity_angular, + acceleration_linear, + dt, + position, + orientation, + velocity_linear, + velocity_angular, + acceleration_linear, + jacobian); + + // Convert back to tf2 representation + pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); + pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w())); + + odom_output.pose.pose.position.x = position.x(); + odom_output.pose.pose.position.y = position.y(); + odom_output.pose.pose.position.z = position.z(); + odom_output.pose.pose.orientation.x = orientation.x(); + odom_output.pose.pose.orientation.y = orientation.y(); + odom_output.pose.pose.orientation.z = orientation.z(); + odom_output.pose.pose.orientation.w = orientation.w(); + + odom_output.twist.twist.linear.x = velocity_linear.x(); + odom_output.twist.twist.linear.y = velocity_linear.y(); + odom_output.twist.twist.linear.z = velocity_linear.z(); + odom_output.twist.twist.angular.x = velocity_angular.x(); + odom_output.twist.twist.angular.y = velocity_angular.y(); + odom_output.twist.twist.angular.z = velocity_angular.z(); + + if (params_.predict_with_acceleration) { + acceleration_output.accel.accel.linear.x = acceleration_linear.x(); + acceleration_output.accel.accel.linear.y = acceleration_linear.y(); + acceleration_output.accel.accel.linear.z = acceleration_linear.z(); + } + + odom_output.header.stamp = timer_now; + acceleration_output.header.stamp = timer_now; + + // Either the last covariance computation was skipped because there was no subscriber, + // or it failed + if (latest_covariance_valid) { + fuse_core::Matrix15d covariance; + covariance.setZero(); + Eigen::Map pose_covariance(odom_output.pose.covariance.data()); + Eigen::Map twist_covariance(odom_output.twist.covariance.data()); + Eigen::Map acceleration_covariance(acceleration_output.accel.covariance.data()); + + covariance.block<6, 6>(0, 0) = pose_covariance; + covariance.block<6, 6>(6, 6) = twist_covariance; + covariance.block<3, 3>(12, 12) = acceleration_covariance; + + covariance = jacobian * covariance * jacobian.transpose(); + + auto process_noise_covariance = params_.process_noise_covariance; + if (params_.scale_process_noise) { + common::scaleProcessNoiseCovariance( + process_noise_covariance, + velocity_linear, + velocity_angular, + params_.velocity_linear_norm_min_, + params_.velocity_angular_norm_min_ + ); + } + + covariance.noalias() += dt * process_noise_covariance; + + pose_covariance = covariance.block<6, 6>(0, 0); + twist_covariance = covariance.block<6, 6>(6, 6); + acceleration_covariance = covariance.block<3, 3>(12, 12); + } } odom_pub_->publish(odom_output); From 49d12a9b8f64b6fa04d5c6fd756dd2220c54f812 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 20 Dec 2023 11:31:00 +0000 Subject: [PATCH 045/116] feature: quat2rpy and quat product jacobian --- fuse_core/include/fuse_core/util.hpp | 95 ++++++++++++++++++- .../fuse_models/unicycle_3d_predict.hpp | 2 +- .../unicycle_3d_state_cost_function.hpp | 4 +- 3 files changed, 97 insertions(+), 4 deletions(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index 4b4651f6d..cab631e68 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -35,6 +35,7 @@ #define FUSE_CORE__UTIL_HPP_ #include +#include #include #include @@ -157,7 +158,7 @@ Eigen::Matrix rotationMatrix2D(const T angle) * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) */ -static inline void quat2rpy(const double * q, double * rpy, double * jacobian = nullptr) +static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr) { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); @@ -230,6 +231,98 @@ static inline void quat2rpy(const double * q, double * rpy, double * jacobian = } } +/** + * @brief Compute product of two quaternions and the function jacobian + * TODO(giafranchini): parametric jacobian computation? Atm this function is only used in + * normal_prior_pose_3d cost function. There we only need the derivatives wrt quaternion W, + * so at the time we are only computing the jacobian wrt W + * + * @param[in] z Pointer to the first quaternion array (4x1) + * @param[in] w Pointer to the second quaternion array (4x1) + * @param[in] z Pointer to the first quaternion array (4x1) + * @param[in] jacobian Pointer to the jacobian matrix (4x4, optional) + */ +static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr) +{ + ceres::QuaternionProduct(z, w, zw); + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + jacobian_map << + z[0], -z[1], -z[2], -z[3], + z[1], z[0], -z[3], z[2], + z[2], z[3], z[0], -z[1], + z[3], -z[2], z[1], z[0]; + } +} + +/** + * @brief Compute quaternion to AngleAxis conversion and the function jacobian + * + * @param[in] q Pointer to the quaternion array (4x1) + * @param[in] angle_axis Pointer to the angle_axis array (3x1) + * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) + */ +static inline void quaternionToAngleAxis(const double * q, double * angle_axis, double * jacobian = nullptr) +{ + ceres::QuaternionToAngleAxis(q, angle_axis); + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + const double & q0 = q[0]; + const double & q1 = q[1]; + const double & q2 = q[2]; + const double & q3 = q[3]; + const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta = std::sqrt(sin_theta2); + const double cos_theta = q0; + + if (std::fpclassify(sin_theta) != FP_ZERO) { + const double two_theta = 2.0 * + (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + jacobian_map(0, 0) = -2.0 * q1 / q_sum2; + jacobian_map(0, 1) = + two_theta / sin_theta + + (2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) - + (q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 2) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 3) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(1, 0) = -2.0 * q2 / q_sum2; + jacobian_map(1, 1) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 2) = + two_theta / sin_theta + + (2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) - + (q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 3) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(2, 0) = -2.0 * q3 / q_sum2; + jacobian_map(2, 1) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 2) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 3) = + two_theta / sin_theta + + (2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) - + (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + } else { + jacobian_map.setZero(); + jacobian_map(1, 1) = 2.0; + jacobian_map(2, 2) = 2.0; + jacobian_map(3, 3) = 2.0; + } + } +} + /** * @brief Create a compound ROS topic name from two components * diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index 71ffc37bd..afbf7cbef 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -581,7 +581,7 @@ inline void predict( double quat[4] = {orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()}; double rpy[3]; double jacobian_quat2rpy[12]; - fuse_core::quat2rpy(quat, rpy, jacobian_quat2rpy); + fuse_core::quaternion2rpy(quat, rpy, jacobian_quat2rpy); // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp index d096fc027..051873d1c 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp @@ -145,8 +145,8 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, double orientation2_rpy[3]; double j1_quat2rpy[12]; double j2_quat2rpy[12]; - fuse_core::quat2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); - fuse_core::quat2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); + fuse_core::quaternion2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); + fuse_core::quaternion2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); predict( parameters[0][0], // position1_x From 5aa23b09abc2c1c91dfa96947f157167443dd9a6 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 20 Dec 2023 11:32:41 +0000 Subject: [PATCH 046/116] feature: add normal_prior_pose_3d --- .../fuse_constraints/normal_prior_pose_3d.hpp | 8 +- .../absolute_pose_3d_stamped_constraint.cpp | 26 ++- fuse_constraints/src/normal_prior_pose_3d.cpp | 112 +++------- fuse_constraints/test/CMakeLists.txt | 1 + .../test/test_normal_prior_pose_3d.cpp | 206 ++++++++++++++++++ 5 files changed, 261 insertions(+), 92 deletions(-) create mode 100644 fuse_constraints/test/test_normal_prior_pose_3d.cpp diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 4318c18b0..92f178112 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -51,10 +51,10 @@ namespace fuse_constraints * * The cost function is of the form: * - * || [ x - b(0)] ||^2 - * cost(x) = || A * [ y - b(1)] || - * || [ z - b(2)] || - * || [ quat2eul(b(3-6)^-1 * q)] || + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2angleaxis(b(3-6)^-1 * q)] || * * Here, the matrix A can be of variable size, thereby permitting the computation of errors for * partial measurements. The vector b is a fixed-size 7x1. In case the user is interested in diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index 1b02e322d..b71bdc482 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,14 +32,15 @@ * 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 namespace fuse_constraints @@ -68,7 +70,6 @@ AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( { // Compute the partial sqrt information matrix of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; // Assemble a sqrt information matrix from the provided values, but in proper Variable order // @@ -86,8 +87,8 @@ AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( } // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - // std::cout << "mean_ = " << mean_.transpose() << std::endl; -} + // std::cout << "mean_ = " << mean_.transpose() << std::endl; + } fuse_core::Matrix6d AbsolutePose3DStampedConstraint::covariance() const { @@ -126,9 +127,18 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_), - sqrt_information_.rows()); + return new NormalPriorPose3D(sqrt_information_, mean_); + + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + + // And including the followings: + // #include + // #include } } // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index f5ef3987a..28200bc2d 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -39,8 +39,6 @@ #include #include -#include - namespace fuse_constraints { @@ -59,22 +57,33 @@ bool NormalPriorPose3D::Evaluate( double ** jacobians) const { fuse_core::Vector6d full_residuals_vector; - full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x - full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y - full_residuals_vector[2] = parameters[0][2] - b_[2]; // position z - // Eigen::Map q_orientation_map(parameters[1]); - auto q_orientation_map = Eigen::Quaterniond(parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3]); - auto qb_inv = Eigen::Quaterniond(b_(3), b_(4), b_(5), b_(6)).conjugate(); - auto q_res = qb_inv * q_orientation_map; - - full_residuals_vector[3] = fuse_core::getRoll(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation roll - full_residuals_vector[4] = fuse_core::getPitch(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation pitch - full_residuals_vector[5] = fuse_core::getYaw(q_res.w(), q_res.x(), q_res.y(), q_res.z()); // orientation yaw + double variable[4] = + { + parameters[1][0], + parameters[1][1], + parameters[1][2], + parameters[1][3], + }; + + double observation_inverse[4] = + { + b_(3), + -b_(4), + -b_(5), + -b_(6) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; - std::cout << "q_res: " << q_res.w() << ", " << q_res.x() << ", " << q_res.y() << ", " << q_res.z() << std::endl; - std::cout << "full_residuals_vector: " << full_residuals_vector.transpose() << std::endl; - + full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x + full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y + full_residuals_vector[2] = parameters[0][2] - b_[2]; // position z + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, &full_residuals_vector[3], j_quat2angle); // orientation angle-axis + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_vector(residuals, num_residuals()); @@ -83,74 +92,17 @@ bool NormalPriorPose3D::Evaluate( if (jacobians != nullptr) { // Jacobian of the position residuals wrt position parameters block (max 3x3) if (jacobians[0] != nullptr) { - Eigen::Map j0_map(jacobians[0], num_residuals(), 3); - j0_map.setZero(); - j0_map.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - // j0_map.leftCols<3>() = A_.block<3, 3>(0, 0); - j0_map.applyOnTheLeft(A_); + Eigen::Map j0_map(jacobians[0], num_residuals(), 3); + j0_map.setIdentity(); + j0_map.applyOnTheLeft(A_.leftCols<3>()); } - // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) if (jacobians[1] != nullptr) { - - Eigen::Map j1_map(jacobians[1], num_residuals(), 4); - j1_map.setZero(); - - // We make use of the chain rule of derivatives: - // First: compute the jacobian of the quaternion product wrt orientation parameters - fuse_core::Matrix4d dqprod_dq1; - dqprod_dq1(0, 0) = qb_inv.w(); - dqprod_dq1(0, 1) = -qb_inv.x(); - dqprod_dq1(0, 2) = -qb_inv.y(); - dqprod_dq1(0, 3) = -qb_inv.z(); - - dqprod_dq1(1, 0) = qb_inv.x(); - dqprod_dq1(1, 1) = qb_inv.w(); - dqprod_dq1(1, 2) = -qb_inv.z(); - dqprod_dq1(1, 3) = qb_inv.y(); - - dqprod_dq1(2, 0) = qb_inv.y(); - dqprod_dq1(2, 1) = qb_inv.z(); - dqprod_dq1(2, 2) = qb_inv.w(); - dqprod_dq1(2, 3) = -qb_inv.x(); - - dqprod_dq1(3, 0) = qb_inv.z(); - dqprod_dq1(3, 1) = -qb_inv.y(); - dqprod_dq1(3, 2) = qb_inv.x(); - dqprod_dq1(3, 3) = qb_inv.w(); - - // Second: compute the jacobian of the quat2eul function wrt the quaternion residual - Eigen::Matrix dquat2eul_dq; - covariance_geometry::jacobianQuaternionToRPY(q_res, dquat2eul_dq); - Eigen::PermutationMatrix<4> perm; - perm.indices() = {1, 2, 3, 0}; - dquat2eul_dq.applyOnTheRight(perm); - - // Third: apply the chain rule - // j1_map.rightCols<3>() = (A_.block<3, 3>(3, 3) * dquat2eul_dq * dqprod_dq1).transpose(); - j1_map.block<3, 4>(3, 0) = dquat2eul_dq * dqprod_dq1; - j1_map.applyOnTheLeft(A_); + Eigen::Map j1_map(jacobians[1], num_residuals(), 4); + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + j1_map = A_.rightCols<3>() * j_quat2angle_map * j_product_map; } - - std::cout << "residuals: " << residuals_vector.transpose() << std::endl; - std::cout << "jacobians[0] : " << std::endl; - for (size_t i = 0; i < 24; i++) - { - if (i % 3 == 0) - std::cout << std::endl; - std::cout << jacobians[0][i] << ", "; - } - std::cout << std::endl; - std::cout << "eigen jacobians[0]: " << std::endl << Eigen::Map(jacobians[0], num_residuals(), 3) << std::endl; - - std::cout << "jacobians[1] : " << std::endl; - for (size_t i = 0; i < 24; i++) - { - if (i % 4 == 0) - std::cout << std::endl; - std::cout << jacobians[1][i] << ", "; - } - std::cout << "eigen jacobians[1]: " << std::endl << Eigen::Map(jacobians[1], num_residuals(), 4) << std::endl; } return true; } diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 09f42f98f..fc7ed7389 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -9,6 +9,7 @@ set(TEST_TARGETS test_marginalize_variables test_normal_delta_pose_2d test_normal_prior_pose_2d + test_normal_prior_pose_3d test_relative_constraint test_relative_pose_2d_stamped_constraint test_relative_pose_3d_stamped_constraint diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp new file mode 100644 index 000000000..8a31ff529 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -0,0 +1,206 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 "cost_function_gtest.hpp" +#include +#include +#include + +/** + * @brief Test fixture that initializes a full pose 3d mean and sqrt information matrix. + */ +class NormalPriorPose3DTestFixture : public ::testing::Test +{ +public: + //!< The automatic differentiation cost function type for the pose 3d cost functor + using AutoDiffNormalPriorPose3D = + ceres::AutoDiffCostFunction; + + /** + * @brief Constructor + */ + NormalPriorPose3DTestFixture() + { + full_sqrt_information = covariance.inverse().llt().matrixU(); + } + + const fuse_core::Matrix6d covariance = + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components + Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< qw, qx, qy, qz +}; + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) +{ + // Create cost function + auto q = Eigen::Quaterniond::UnitRandom(); + full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); // Create automatic differentiation cost function + const fuse_constraints::NormalPriorPose3D cost_function{full_sqrt_information, full_mean}; + const auto num_residuals = full_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), + num_residuals); + + // Compare the expected, automatic differentiation, cost function and the actual one + // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function + // and the second argument is the actual cost function + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) +{ + // Create cost function for a subset of residuals + // Version with y position = 0 + std::vector indices = {0, 2, 3, 4, 5}; + Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + + q = Eigen::Quaterniond::UnitRandom(); + full_mean << 1.0, 0.0, 1.0, q.w(), q.x(), q.y(), q.z(); + fuse_core::Matrix partial_sqrt_information; + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) +{ + // Create cost function for a subset of residuals + // Version with roll, pitch = 0 + std::vector indices = {0, 1, 2, 5}; + Eigen::Vector3d rpy {0.0, 0.0, 0.1}; + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) +{ + // Create cost function for a subset of residuals + // Version with position = 0, roll = 0 + std::vector indices = {4, 5}; + Eigen::Vector3d rpy {0.0, -0.2, 0.1}; + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + full_mean << 0.0, 0.0, 0.0, q.w(), q.x(), q.y(), q.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) +{ + // Create cost function for a subset of residuals + // Version with z = 0, orientation = 0 + std::vector indices = {0, 1}; + Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + full_mean << 0.1, 0.5, 0.0, q.w(), q.x(), q.y(), q.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} From 0ae0f4cf95175455c16a2ab42fa2ebbd27454bb4 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 20 Dec 2023 11:34:13 +0000 Subject: [PATCH 047/116] process only orientation for imu3d --- .../fuse_models/common/sensor_proc.hpp | 233 ++++++++++++++++-- .../fuse_models/parameters/imu_3d_params.hpp | 2 +- fuse_models/src/imu_3d.cpp | 3 +- 3 files changed, 218 insertions(+), 20 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 2f315da20..05ba9ed25 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -498,27 +499,68 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - // Convert the ROS message into tf2 transform - tf2::Transform tf2_pose; - tf2::fromMsg(transformed_message.pose.pose, tf2_pose); - // Create the pose variable auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); - position->x() = tf2_pose.getOrigin().x(); - position->y() = tf2_pose.getOrigin().y(); - position->z() = tf2_pose.getOrigin().z(); + position->x() = transformed_message.pose.pose.position.x; + position->y() = transformed_message.pose.pose.position.y; + position->z() = transformed_message.pose.pose.position.z; auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); - orientation->x() = tf2_pose.getRotation().x(); - orientation->y() = tf2_pose.getRotation().y(); - orientation->z() = tf2_pose.getRotation().z(); - orientation->w() = tf2_pose.getRotation().w(); + orientation->w() = transformed_message.pose.pose.orientation.w; + orientation->x() = transformed_message.pose.pose.orientation.x; + orientation->y() = transformed_message.pose.pose.orientation.y; + orientation->z() = transformed_message.pose.pose.orientation.z; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Use the full pose measurement, no need to create a partial measurement + fuse_core::Vector7d pose_mean; + pose_mean << transformed_message.pose.pose.position.x, + transformed_message.pose.pose.position.y, + transformed_message.pose.pose.position.z, + transformed_message.pose.pose.orientation.w, + transformed_message.pose.pose.orientation.x, + transformed_message.pose.pose.orientation.y, + transformed_message.pose.pose.orientation.z; + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + if (validate) { + try { + validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( + source, + *position, + *orientation, + pose_mean, + pose_covariance); + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + return true; + } + + // Convert the ROS message into tf2 transform + tf2::Transform tf2_pose; + tf2::fromMsg(transformed_message.pose.pose, tf2_pose); // Fill eigen pose in RPY representation fuse_core::Vector6d pose_mean; pose_mean.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean(3), pose_mean(4), pose_mean(5)); - Eigen::Map pose_covariance_map(transformed_message.pose.covariance.data()); + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); // Set the components which are not measured to zero const auto indices = mergeIndices(position_indices, orientation_indices, 3); @@ -529,7 +571,7 @@ inline bool processAbsolutePose3DWithCovariance( }, 0.0); fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); - populatePartialMeasurement(pose_covariance_map, indices, pose_covariance_partial); + populatePartialMeasurement(pose_covariance, indices, pose_covariance_partial); tf2::Quaternion q_partial; q_partial.setRPY(pose_mean(3), pose_mean(4), pose_mean(5)); @@ -569,6 +611,165 @@ inline bool processAbsolutePose3DWithCovariance( return true; } +/** + * @brief Extracts 3D orientation data from a PoseWithCovarianceStamped message and adds that data to a + * fuse Transaction + * + * This method effectively adds a 3D orientation variable and a 3D orientation + * constraint to the given \p transaction. The orientation data is extracted from the \p pose message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose - The PoseWithCovarianceStamped message from which we will extract the orientation data + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] target_frame - The frame ID into which the pose data will be transformed before it is + * used + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAbsoluteOrientation3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & orientation_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + if (orientation_indices.empty()) { + return false; + } + + geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = pose; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + // Create the orientation variable + auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + orientation->w() = transformed_message.pose.pose.orientation.w; + orientation->x() = transformed_message.pose.pose.orientation.x; + orientation->y() = transformed_message.pose.pose.orientation.y; + orientation->z() = transformed_message.pose.pose.orientation.z; + + if (orientation_indices.size() == 3) { + // Use the full orientation measurement, no need to create a partial measurement + fuse_core::Vector4d orientation_mean; + orientation_mean << + transformed_message.pose.pose.orientation.w, + transformed_message.pose.pose.orientation.x, + transformed_message.pose.pose.orientation.y, + transformed_message.pose.pose.orientation.z; + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + fuse_core::Matrix3d orientation_covariance = pose_covariance.block<3,3>(3,3); + + if (validate) { + try { + validatePartialMeasurement(orientation_mean, orientation_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( + source, + *orientation, + orientation_mean, + orientation_covariance); + + constraint->loss(loss); + + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; + } + + // TODO(giafranchini): in case of partial measurements, is probably better to use the + // AbsoluteOrientation3DStampedEulerConstraint, since already implements support for these. + + // Convert the ROS message into tf2 quaternion + tf2::Quaternion q; + tf2::fromMsg(transformed_message.pose.pose.orientation, q); + // Fill eigen orientation in RPY representation + fuse_core::Vector3d orientation_mean; + tf2::Matrix3x3(q).getRPY(orientation_mean(0), orientation_mean(1), orientation_mean(2)); + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + fuse_core::Matrix3d orientation_covariance = pose_covariance.block<3,3>(3,3); + + // Set the components which are not measured to zero + std::replace_if( + orientation_mean.data(), orientation_mean.data() + orientation_mean.size(), + [&orientation_indices, &orientation_mean](const double & value) { + return std::find( + orientation_indices.begin(), + orientation_indices.end(), + &value - orientation_mean.data()) == orientation_indices.end(); + }, + 0.0); + + fuse_core::MatrixXd orientation_covariance_partial(orientation_indices.size(), orientation_indices.size()); + populatePartialMeasurement(orientation_covariance, orientation_indices, orientation_covariance_partial); + + tf2::Quaternion q_partial; + q_partial.setRPY(orientation_mean(0), orientation_mean(1), orientation_mean(2)); + + // Create the orientation for the constraint + fuse_core::Vector4d orientation_mean_partial; + orientation_mean_partial << q_partial.getW(), q_partial.getX(), q_partial.getY(), q_partial.getZ(); + + if (validate) { + try { + validatePartialMeasurement(orientation_mean_partial, orientation_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( + source, + *orientation, + orientation_mean_partial, + orientation_covariance_partial, + orientation_indices); + + constraint->loss(loss); + + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -977,7 +1178,7 @@ inline bool processDifferentialPose3DWithCovariance( const bool validate, fuse_core::Transaction & transaction) { - // TODO:: we should probably remove covariance_geometry dependency + // TODO(giafranchini): we should probably remove covariance_geometry dependency // Convert from ROS msg to covariance geometry types // PoseQuaternionCovarianceRPY is std::pair, Covariance> @@ -1019,7 +1220,7 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } else { - // TODO: check this method, results are nosense + // TODO(giafranchini): check this method, results are nosense // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. // We know cov1 and cov2 and we should substract the first to the second in order // to obtain the relative pose covariance. But first the 2 of them have to be in the @@ -1646,7 +1847,6 @@ inline bool processTwist3DWithCovariance( transformed_message.twist.twist.linear.z; // Create the covariance for the constraint - // TODO: check how to only map linear 3x3 part of the covariance Eigen::Map linear_vel_covariance_map( transformed_message.twist.covariance.data()); @@ -1705,7 +1905,6 @@ inline bool processTwist3DWithCovariance( transformed_message.twist.twist.angular.z; // Create the covariance for the constraint - // TODO: check how to only map angular 3x3 part of the covariance Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 1aca86f61..e388fe225 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -130,7 +130,7 @@ struct Imu3DParams : public ParameterBase ns, "independent"), independent); - // TODO: understand if this is needed + // TODO(giafranchini): understand if this is needed // use_twist_covariance = // fuse_core::getParam( // interfaces, fuse_core::joinParameterName( diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index f20f5d838..54c689837 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -173,13 +173,12 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) if (params_.differential) { processDifferential(*pose, validate, *transaction); } else { - common::processAbsolutePose3DWithCovariance( + common::processAbsoluteOrientation3DWithCovariance( name(), device_id_, *pose, params_.pose_loss, params_.orientation_target_frame, - {}, params_.orientation_indices, *tf_buffer_, validate, From 7a8cde819cf08ff0cbb958f8d8c50cf89a3572c5 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 20 Dec 2023 13:16:23 +0000 Subject: [PATCH 048/116] feature: add normal_prior_orientation_3d --- fuse_constraints/CMakeLists.txt | 1 + .../normal_prior_orientation_3d.hpp | 106 ++++++++++++++++++ ...lute_orientation_3d_stamped_constraint.cpp | 10 ++ .../src/normal_prior_orientation_3d.cpp | 99 ++++++++++++++++ fuse_models/test/test_sensor_proc.cpp | 2 +- 5 files changed, 217 insertions(+), 1 deletion(-) create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp create mode 100644 fuse_constraints/src/normal_prior_orientation_3d.cpp diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index f8fe221cb..e2b8324d0 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -45,6 +45,7 @@ add_library(${PROJECT_NAME} src/normal_delta_orientation_2d.cpp src/normal_delta_pose_2d.cpp src/normal_prior_orientation_2d.cpp + src/normal_prior_orientation_3d.cpp src/normal_prior_pose_2d.cpp src/normal_prior_pose_3d.cpp src/relative_constraint.cpp diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp new file mode 100644 index 000000000..3be895a8d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on a 3D orientation variable (quaternion) + * + * The cost function is of the form: + * + * || ||^2 + * cost(x) = || A * AngleAxis(b^-1 * q) || + * || || + * + * where the matrix A and the vector b are fixed, and q is the variable being measured, represented + * as a quaternion. The AngleAxis function converts a quaternion into a 3-vector of the form + * theta*k, where k is the unit vector axis of rotation and theta is the angle of rotation. The A + * matrix is applied to the angle-axis 3-vector. + * + * In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorOrientation3D : public ceres::SizedCostFunction +{ +public: + /** + * @brief Construct a cost function instance + * + * The residual weighting matrix can vary in size, as this cost functor can be used to compute + * costs for partial vectors. The number of rows of A will be the number of dimensions for which + * you want to compute the error, and the number of columns in A will be fixed at 3. For example, + * if we just want to use the values of roll and yaw, then \p A will be of size 2x3. + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (roll, pitch, yaw) + * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) + */ + NormalPriorOrientation3D(const fuse_core::MatrixXd & A, const fuse_core::Vector4d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorOrientation3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector4d b_; //!< The measured 3D orientation value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index c6d0c8e1e..e4cb8ede2 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -38,6 +38,7 @@ #include #include +// #include #include #include @@ -135,9 +136,18 @@ void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsoluteOrientation3DStampedConstraint::costFunction() const { + // return new NormalPriorOrientation3D(sqrt_information_, mean_); + + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + return new ceres::AutoDiffCostFunction( new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_), sqrt_information_.rows()); + + // And including the followings: + // #include + // #include } fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp new file mode 100644 index 000000000..176057d80 --- /dev/null +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 + +namespace fuse_constraints +{ + +NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::MatrixXd & A, const fuse_core::Vector4d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 3); + set_num_residuals(A_.rows()); +} + +bool NormalPriorOrientation3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + fuse_core::Vector3d full_residuals_vector; + + double variable[4] = + { + parameters[0][0], + parameters[0][1], + parameters[0][2], + parameters[0][3], + }; + + double observation_inverse[4] = + { + b_(0), + -b_(1), + -b_(2), + -b_(3) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; + + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, &full_residuals_vector[0], j_quat2angle); // orientation angle-axis + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_vector(residuals, num_residuals()); + residuals_vector = A_ * full_residuals_vector; + + if (jacobians != nullptr) { + // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) + Eigen::Map j_map(jacobians[0], num_residuals(), 4); + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + j_map = A_ * j_quat2angle_map * j_product_map; + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 637a93699..7377f6626 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -11,7 +11,7 @@ #include namespace fm_common = fuse_models::common; - +//TODO(giafranchini): Add tests for 3d sensors TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) { const std::vector position_indices{0, 1}; From 00ecb7ef830aaae0d20d31aea5b1d89ee3af67f6 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 20 Dec 2023 17:25:03 +0000 Subject: [PATCH 049/116] fix: normal prior pose jacobian maps --- fuse_constraints/src/normal_prior_pose_3d.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 28200bc2d..ae96b6815 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -90,17 +90,15 @@ bool NormalPriorPose3D::Evaluate( residuals_vector = A_ * full_residuals_vector; if (jacobians != nullptr) { - // Jacobian of the position residuals wrt position parameters block (max 3x3) + // Jacobian of the residuals wrt position parameter block if (jacobians[0] != nullptr) { - Eigen::Map j0_map(jacobians[0], num_residuals(), 3); - j0_map.setIdentity(); - j0_map.applyOnTheLeft(A_.leftCols<3>()); + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); } - // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) + // Jacobian of the residuals wrt orientation parameter block if (jacobians[1] != nullptr) { - Eigen::Map j1_map(jacobians[1], num_residuals(), 4); Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); + Eigen::Map j1_map(jacobians[1], num_residuals(), 4); j1_map = A_.rightCols<3>() * j_quat2angle_map * j_product_map; } } From e9f1aa454e5b0706dee672a5af66f8610db8d258 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 28 Dec 2023 21:24:43 +0000 Subject: [PATCH 050/116] dev: sensor_proc refactor --- fuse_constraints/CMakeLists.txt | 2 + fuse_constraints/fuse_plugins.xml | 6 + .../absolute_pose_3d_stamped_constraint.hpp | 23 +- ...olute_pose_3d_stamped_euler_constraint.hpp | 197 +++++++ ...rior_orientation_3d_euler_cost_functor.hpp | 53 +- .../fuse_constraints/normal_prior_pose_3d.hpp | 15 +- .../normal_prior_pose_3d_cost_functor.hpp | 23 +- .../normal_prior_pose_3d_euler.hpp | 107 ++++ ...ormal_prior_pose_3d_euler_cost_functor.hpp | 135 +++++ ...ative_pose_3d_stamped_euler_constraint.hpp | 190 +++++++ .../absolute_pose_2d_stamped_constraint.cpp | 3 - .../absolute_pose_3d_stamped_constraint.cpp | 53 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 145 ++++++ fuse_constraints/src/normal_prior_pose_3d.cpp | 29 +- .../src/normal_prior_pose_3d_euler.cpp | 95 ++++ ...ative_pose_3d_stamped_euler_constraint.cpp | 140 +++++ fuse_constraints/test/CMakeLists.txt | 2 + ...rientation_3d_stamped_euler_constraint.cpp | 6 +- ...st_absolute_pose_3d_stamped_constraint.cpp | 197 ------- ...olute_pose_3d_stamped_euler_constraint.cpp | 485 ++++++++++++++++++ .../test/test_normal_prior_pose_3d.cpp | 117 +---- .../test/test_normal_prior_pose_3d_euler.cpp | 195 +++++++ .../fuse_models/common/sensor_proc.hpp | 230 ++------- fuse_models/src/imu_3d.cpp | 3 +- ...unicycle_3d_state_kinematic_constraint.cpp | 2 +- fuse_models/test/utils.py | 93 ++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 2 + 27 files changed, 1889 insertions(+), 659 deletions(-) create mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/src/normal_prior_pose_3d_euler.cpp create mode 100644 fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp create mode 100644 fuse_models/test/utils.py diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index e2b8324d0..0be5d3887 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -38,6 +38,7 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp + src/absolute_pose_3d_stamped_euler_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -48,6 +49,7 @@ add_library(${PROJECT_NAME} src/normal_prior_orientation_3d.cpp src/normal_prior_pose_2d.cpp src/normal_prior_pose_3d.cpp + src/normal_prior_pose_3d_euler.cpp src/relative_constraint.cpp src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index 7f23443ce..a53a5762b 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -87,6 +87,12 @@ A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose. + + + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose, + orientation parametrized in RPY. + + A constraint that represents remaining marginal information on a set of variables. diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 2727fd6f8..72fd9d016 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -95,25 +95,6 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint const fuse_core::Vector7d & mean, const fuse_core::Matrix6d & covariance); - /** - * @brief Create a constraint using a measurement/prior of the 3D pose (version for partial measurements) - * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] position The variable representing the position components of the pose - * @param[in] orientation The variable representing the orientation components of the pose - * @param[in] mean The measured/prior pose as a vector - * (7x1 vector: x, y, z, qw, qx, qy, qz) - * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) - * @param[in] variable_indices The indices of the measured variables - */ - AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); - /** * @brief Destructor */ @@ -131,7 +112,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qx, qy, qz) */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. @@ -161,7 +142,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint protected: fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable - fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix private: // Allow Boost Serialization access to private methods diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..41b88a402 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,197 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents either prior information about a 3D pose, or a direct + * measurement of the 3D pose. + * + * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, + * this class applies an absolute constraint on both variables at once. This type of constraint + * arises in many situations. In mapping it is common to define the very first pose as the origin. + * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. + * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map + * measurements). This constraint holds the measured 3D pose: orientation is parametrized as roll-pitch-yaw + * Euler angles and the covariance represents the error around each translational and rotational axis. + * This constraint allows measurement of a subset of the pose components given in the variable. + * + * (Use AbsolutePose3DStampedConstraint for full pose measurements). + */ + +class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint) + + /** + * @brief Default constructor + */ + AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] partial_mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + * @param[in] variable_indices The indices of the measured variables + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::Vector6d & mean() const {return mean_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint 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 Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & mean_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index e91a6a59b..fd49f06c5 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -83,12 +83,12 @@ class NormalPriorOrientation3DEulerCostFunctor * * @param[in] A The residual weighting matrix, most likely the square root information matrix. Its * order must match the values in \p axes. - * @param[in] b The orientation measurement or prior. + * @param[in] b The orientation measurement or prior. Its order must match the values in \p axes. * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. */ NormalPriorOrientation3DEulerCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b, + const fuse_core::MatrixXd & A, + const fuse_core::VectorXd & b, const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}) : //NOLINT A_(A), b_(b), @@ -104,46 +104,26 @@ class NormalPriorOrientation3DEulerCostFunctor { using fuse_variables::Orientation3DStamped; - // Compute the delta quaternion - // T variable[4] = - // { - // orientation[0], - // orientation[1], - // orientation[2], - // orientation[3] - // }; - - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; - - T difference[4]; - ceres::QuaternionProduct(observation_inverse, orientation, difference); - - T angle[3] = {T(0.0), T(0.0), T(0.0)}; - for (size_t i = 0; i < axes_.size(); ++i) { + T angle; switch (axes_[i]) { case Euler::ROLL: { - angle[0] = fuse_core::getRoll( - difference[0], difference[1], difference[2], difference[3]); + angle = fuse_core::getRoll( + orientation[0], orientation[1], orientation[2], + orientation[3]); break; } case Euler::PITCH: { - angle[1] = fuse_core::getPitch( - difference[0], difference[1], difference[2], difference[3]); + angle = + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); break; } case Euler::YAW: { - angle[2] = fuse_core::getYaw( - difference[0], difference[1], difference[2], difference[3]); + angle = + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); break; } default: @@ -153,22 +133,19 @@ class NormalPriorOrientation3DEulerCostFunctor "I should probably be more informative here"); } } + residuals[i] = angle - T(b_[i]); } - residuals[0] = angle[0]; - residuals[1] = angle[1]; - residuals[2] = angle[2]; - - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map.applyOnTheLeft(A_.template cast()); return true; } private: - fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root //!< information matrix - fuse_core::Vector4d b_; //!< The measured 3D orientation (quaternion) value + fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value std::vector axes_; //!< The Euler angle axes that we're measuring }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 92f178112..7ec76458c 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -56,31 +56,24 @@ namespace fuse_constraints * || [ z - b(2)] || * || [ quat2angleaxis(b(3-6)^-1 * q)] || * - * Here, the matrix A can be of variable size, thereby permitting the computation of errors for - * partial measurements. The vector b is a fixed-size 7x1. In case the user is interested in - * implementing a cost function of the form: + * In case the user is interested in implementing a cost function of the form: * * cost(X) = (X - mu)^T S^{-1} (X - mu) * * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the * square root information matrix (the inverse of the covariance). */ -class NormalPriorPose3D : public ceres::SizedCostFunction +class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> { public: /** * @brief Construct a cost function instance * - * The residual weighting matrix can vary in size, as this cost functor can be used to compute - * costs for partial vectors. The number of rows of A will be the number of dimensions for which - * you want to compute the error, and the number of columns in A will be fixed at 6. For example, - * if we just want to use the values of x, y and yaw, then \p A will be of size 3x6. - * * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, z, roll, pitch, yaw) * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3D(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); + NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); /** * @brief Destructor @@ -97,7 +90,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction double ** jacobians) const; private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix fuse_core::Vector7d b_; //!< The measured 3D pose value }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index 5f536eb17..99e9b119f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyrigth (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -80,7 +81,7 @@ class NormalPriorPose3DCostFunctor * order (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); + NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -89,21 +90,19 @@ class NormalPriorPose3DCostFunctor bool operator()(const T * const position, const T * const orientation, T * residual) const; private: - fuse_core::MatrixXd A_; + fuse_core::Matrix6d A_; fuse_core::Vector7d b_; NormalPriorOrientation3DCostFunctor orientation_functor_; }; NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor( - const fuse_core::MatrixXd & A, + const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { - CHECK_GT(A_.rows(), 0); - CHECK_EQ(A_.cols(), 6); } template @@ -111,20 +110,18 @@ bool NormalPriorPose3DCostFunctor::operator()( const T * const position, const T * const orientation, T * residual) const { - T full_residuals[6]; // Compute the position error - full_residuals[0] = position[0] - T(b_(0)); - full_residuals[1] = position[1] - T(b_(1)); - full_residuals[2] = position[2] - T(b_(2)); + residual[0] = position[0] - T(b_(0)); + residual[1] = position[1] - T(b_(1)); + residual[2] = position[2] - T(b_(2)); // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation, &full_residuals[3]); + orientation_functor_(orientation, &residual[3]); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> full_residuals_map(full_residuals); - Eigen::Map> residuals_map(residual, A_.rows()); - residuals_map = A_.template cast() * full_residuals_map; + Eigen::Map> residuals_map(residual); + residuals_map.applyOnTheLeft(A_.template cast()); return true; } diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp new file mode 100644 index 000000000..4d8fce419 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2eul(q) - b(3:5) ] || + * + * Here, the matrix A can be of variable size, thereby permitting the computation of errors for + * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in + * implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEuler : public ceres::SizedCostFunction +{ +public: + /** + * @brief Construct a cost function instance + * + * The residual weighting matrix can vary in size, as this cost functor can be used to compute + * costs for partial vectors. The number of rows of A will be the number of dimensions for which + * you want to compute the error, and the number of columns in A will be fixed at 6. For example, + * if we just want to use the values of x, y and yaw, then \p A will be of size 3x6. + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3DEuler() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector6d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..e6eb471bb --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the 3D position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the 3D position and orientation variables at + * once. + * + * The cost function is of the form: + * + * cost(x) = || A * [ p - b(0:2) ] ||^2 + * || [ quat2eul(q) - b(3:5) ] || + * + * where, the matrix A and the vector b are fixed, p is the position variable, and q is the + * orientation variable. In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T * const position, const T * const orientation, T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector6d b_; +}; + +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); +} + +template +bool NormalPriorPose3DEulerCostFunctor::operator()( + const T * const position, const T * const orientation, + T * residual) const +{ + T full_residuals[6]; + T orientation_rpy[3] = { + fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) + }; + + // Compute the position residual + full_residuals[0] = position[0] - T(b_(0)); + full_residuals[1] = position[1] - T(b_(1)); + full_residuals[2] = position[2] - T(b_(2)); + // Compute the orientation residual + full_residuals[3] = orientation_rpy[0] - T(b_(3)); + full_residuals[4] = orientation_rpy[1] - T(b_(4)); + full_residuals[5] = orientation_rpy[2] - T(b_(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..5d9d90fe3 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +// TODO(giafranchini): still to be implemented + +#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents a measurement on the difference between two 3D poses. + * + * This type of constraint arises in many situations. Many types of incremental odometry + * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This + * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. + */ +class RelativePose3DStampedConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedConstraint) + + /** + * @brief Default constructor + */ + RelativePose3DStampedConstraint() = default; + + /** + * @brief Constructor + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose + * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) + */ + RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Constructor (version for partial measurements) + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose + * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] variable_indices The indices of the measured variables + */ + RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + */ + const fuse_core::Vector7d & delta() const {return delta_;} + + /** + * @brief Read-only access to the square root information matrix. + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + */ + fuse_core::MatrixXd covariance() const; + + /** + * @brief Print a human-readable description of the constraint 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 Access the cost function for this constraint + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the + //!< covariance matrix) + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & delta_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedConstraint); + +#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 7fcdebae2..1b84d5f1b 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -64,7 +64,6 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( // Compute the sqrt information of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; // Assemble a mean vector and sqrt information matrix from the provided values, but in proper // Variable order @@ -86,8 +85,6 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( mean_(final_index) = partial_mean(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); } - std::cout << "mean_: \n" << mean_.transpose() << std::endl; - std::cout << "sqrt_information_: \n" << sqrt_information_ << std::endl; } fuse_core::Matrix3d AbsolutePose2DStampedConstraint::covariance() const diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index b71bdc482..e2454f3ad 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -58,57 +58,11 @@ AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( { } -AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT - mean_(mean) -{ - // Compute the partial sqrt information matrix of the provided cov matrix - fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - - // Assemble a sqrt information matrix from the provided values, but in proper Variable order - // - // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 - // If we are measuring a subset of dimensions, we only want to produce costs for the measured - // dimensions. But the variable vectors will be full sized. We can make this all work out by - // creating a non-square A, where each row computes a cost for one measured dimensions, - // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions - sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); - for (size_t i = 0; i < variable_indices.size(); ++i) - { - sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); - } - - // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - // std::cout << "mean_ = " << mean_.transpose() << std::endl; - } - fuse_core::Matrix6d AbsolutePose3DStampedConstraint::covariance() const { - if (sqrt_information_.rows() == 6) - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } - // Otherwise we need to compute the pseudoinverse - // cov = (sqrt_info' * sqrt_info)^-1 - // With some linear algebra, we can swap the transpose and the inverse. - // cov = (sqrt_info^-1) * (sqrt_info^-1)' - // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. - // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). - // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. - auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); - fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); - return pinv * pinv.transpose(); + return (sqrt_information_.transpose() * sqrt_information_).inverse(); } - void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const { stream << type() << "\n" @@ -132,9 +86,8 @@ ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: - // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DCostFunctor(sqrt_information_, mean_), - // sqrt_information_.rows()); + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DCostFunctor(sqrt_information_, mean_); // And including the followings: // #include diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..fbe20b11a --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 + +namespace fuse_constraints +{ + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(mean), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(partial_mean) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + + +void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable: " << variables().at(0) << "\n" + << " orientation variable: " << variables().at(1) << "\n" + << " mean: " << mean().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; + + if (loss()) { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +{ + return new NormalPriorPose3DEuler(sqrt_information_, mean_); + + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + + // And including the followings: + // #include + // #include +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 28200bc2d..82df17c00 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -42,13 +42,10 @@ namespace fuse_constraints { -NormalPriorPose3D::NormalPriorPose3D(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b) +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) : A_(A), b_(b) { - CHECK_GT(A_.rows(), 0); - CHECK_EQ(A_.cols(), 6); - set_num_residuals(A_.rows()); } bool NormalPriorPose3D::Evaluate( @@ -56,7 +53,6 @@ bool NormalPriorPose3D::Evaluate( double * residuals, double ** jacobians) const { - fuse_core::Vector6d full_residuals_vector; double variable[4] = { @@ -78,29 +74,28 @@ bool NormalPriorPose3D::Evaluate( double j_product[16]; double j_quat2angle[12]; - full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x - full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y - full_residuals_vector[2] = parameters[0][2] - b_[2]; // position z + residuals[0] = parameters[0][0] - b_[0]; // position x + residuals[1] = parameters[0][1] - b_[1]; // position y + residuals[2] = parameters[0][2] - b_[2]; // position z fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, &full_residuals_vector[3], j_quat2angle); // orientation angle-axis + fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map residuals_vector(residuals, num_residuals()); - residuals_vector = A_ * full_residuals_vector; + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); if (jacobians != nullptr) { - // Jacobian of the position residuals wrt position parameters block (max 3x3) + // Jacobian of the residuals wrt position parameters block if (jacobians[0] != nullptr) { - Eigen::Map j0_map(jacobians[0], num_residuals(), 3); - j0_map.setIdentity(); - j0_map.applyOnTheLeft(A_.leftCols<3>()); + Eigen::Map> j0_map(jacobians[0]); + j0_map = A_.leftCols<3>(); } - // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) + // Jacobian of the residuals wrt orientation parameters block if (jacobians[1] != nullptr) { - Eigen::Map j1_map(jacobians[1], num_residuals(), 4); Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); + Eigen::Map> j1_map(jacobians[1]); j1_map = A_.rightCols<3>() * j_quat2angle_map * j_product_map; } } diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp new file mode 100644 index 000000000..15e340bf0 --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 + +namespace fuse_constraints +{ + +NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); + set_num_residuals(A_.rows()); +} + +bool NormalPriorPose3DEuler::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + fuse_core::Vector6d full_residuals; + double orientation_rpy[3]; + double j_quat2rpy[12]; + + // Compute the position residual + full_residuals(0) = parameters[0][0] - b_(0); + full_residuals(1) = parameters[0][1] - b_(1); + full_residuals(2) = parameters[0][2] - b_(2); + + // Compute the orientation residual + fuse_core::quaternion2rpy(parameters[1], orientation_rpy, j_quat2rpy); + full_residuals(3) = orientation_rpy[0] - b_(3); + full_residuals(4) = orientation_rpy[1] - b_(4); + full_residuals(5) = orientation_rpy[2] - b_(5); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map = A_ * full_residuals; + + if (jacobians != nullptr) { + // Jacobian wrt position + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + // Jacobian wrt orientation + if (jacobians[1] != nullptr) { + Eigen::Map> j_quat2angle_map(j_quat2rpy); + Eigen::Map(jacobians[1], num_residuals(), 4) = + A_.rightCols<3>() * j_quat2angle_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..6c5912535 --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +// TODO(giafranchini): still to be implemented +#include + +#include + +#include +#include +#include +#include + +namespace fuse_constraints +{ + +RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(delta), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector7d & delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(delta) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } + // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; + // std::cout << "mean_ = " << mean_.transpose() << std::endl; +} + +fuse_core::MatrixXd RelativePose3DStampedConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void RelativePose3DStampedConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position1 variable: " << variables().at(0) << "\n" + << " orientation1 variable: " << variables().at(1) << "\n" + << " position2 variable: " << variables().at(2) << "\n" + << " orientation2 variable: " << variables().at(3) << "\n" + << " delta: " << delta().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_), + sqrt_information_.rows()); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index fc7ed7389..c60ecff64 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -5,11 +5,13 @@ set(TEST_TARGETS test_absolute_orientation_3d_stamped_euler_constraint test_absolute_pose_2d_stamped_constraint test_absolute_pose_3d_stamped_constraint + test_absolute_pose_3d_stamped_euler_constraint test_marginal_constraint test_marginalize_variables test_normal_delta_pose_2d test_normal_prior_pose_2d test_normal_prior_pose_3d + test_normal_prior_pose_3d_euler test_relative_constraint test_relative_pose_2d_stamped_constraint test_relative_pose_3d_stamped_constraint diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 3efab9e57..ab685ef3a 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -145,7 +145,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); @@ -220,7 +221,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index f4841c0a3..9aa40bcf1 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -80,33 +80,6 @@ TEST(AbsolutePose3DStampedConstraint, Constructor) mean, cov)); } -TEST(AbsolutePose3DStampedConstraint, ConstructorPartial) -{ - // Construct a constraint just to make sure it compiles. - Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); - - std::vector variable_indices {0, 2, 3, 4, 5}; - - fuse_core::Vector7d mean_partial; - mean_partial << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; - - fuse_core::Matrix5d cov_partial; - /* *INDENT-OFF* */ - cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT - /* *INDENT-ON* */ - - EXPECT_NO_THROW( - AbsolutePose3DStampedConstraint constraint( - "test", position_variable, orientation_variable, - mean_partial, cov_partial, variable_indices)); -} - TEST(AbsolutePose3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct @@ -149,57 +122,6 @@ TEST(AbsolutePose3DStampedConstraint, Covariance) EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); } -TEST(AbsolutePose3DStampedConstraint, CovariancePartial) -{ - // Verify the covariance <--> sqrt information conversions are correct - Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); - - std::vector variable_indices {0, 2, 3, 4, 5}; - - fuse_core::Vector7d mean; - mean << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; - - // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the - // required precision) - fuse_core::Matrix5d cov; - /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT - /* *INDENT-ON* */ - - AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, - mean, cov, variable_indices); - - // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') - fuse_core::Matrix expected_sqrt_info; - /* *INDENT-OFF* */ - expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT - 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT - 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT - 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT - 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT - /* *INDENT-ON* */ - - fuse_core::Matrix6d expected_cov; - /* *INDENT-OFF* */ - expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT - /* *INDENT-ON* */ - - // Compare - EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); - EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-8); -} - TEST(AbsolutePose3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are @@ -316,125 +238,6 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); } -TEST(AbsolutePose3DStampedConstraint, OptimizationPartial) -{ - // Optimize a single pose and single constraint, verify the expected value and covariance are - // generated. Create the variables. Version for partial measurements - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); - position_variable->x() = 1.5; - position_variable->y() = 0.0; - position_variable->z() = 10.0; - - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); - orientation_variable->w() = 0.952; - orientation_variable->x() = 0.038; - orientation_variable->y() = -0.189; - orientation_variable->z() = 0.239; - - // Create an absolute pose constraint - std::vector variable_indices {0, 2, 3, 4, 5}; - - fuse_core::Vector7d mean; - mean << 1.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0; - - fuse_core::Matrix5d cov; - /* *INDENT-OFF* */ - cov << 1.0, 0.2, 0.3, 0.4, 0.5, - 0.2, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.2, 0.4, 0.5, 6.0; - /* *INDENT-ON* */ - - auto constraint = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov, - variable_indices); - - // Build the problem - ceres::Problem::Options problem_options; - problem_options.loss_function_ownership = fuse_core::Loss::Ownership; - ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), - position_variable->localParameterization()); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), - orientation_variable->localParameterization()); - - std::vector parameter_blocks; - parameter_blocks.push_back(position_variable->data()); - parameter_blocks.push_back(orientation_variable->data()); - - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); - - // Run the solver - ceres::Solver::Options options; - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); - - // Check - EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); - EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); - EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); - EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); - EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); - EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); - EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); - - // Compute the covariance - std::vector> covariance_blocks; - covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); - covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); - covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); - - ceres::Covariance::Options cov_options; - cov_options.algorithm_type = ceres::DENSE_SVD; - ceres::Covariance covariance(cov_options); - covariance.Compute(covariance_blocks, &problem); - fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), cov_pos_pos.data()); - - fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); - - fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), orientation_variable->data(), cov_pos_or.data()); - - // Assemble the full covariance from the covariance blocks - fuse_core::Matrix6d actual_covariance; - actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; - - // Define the expected covariance - fuse_core::Matrix6d expected_covariance; - /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; - /* *INDENT-ON* */ - - EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); -} - TEST(AbsolutePose3DStampedConstraint, Serialization) { // Construct a constraint diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..d994f296d --- /dev/null +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,485 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; + + +TEST(AbsolutePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean, cov)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean_partial; + mean_partial << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + fuse_core::Matrix5d cov_partial; + /* *INDENT-OFF* */ + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean, cov, variable_indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT + 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT + 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT + 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + /* *INDENT-ON* */ + + fuse_core::Matrix6d expected_cov; + /* *INDENT-OFF* */ + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-8); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables. Version for partial measurements + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = 0.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.2, 0.3, 0.4, 0.5, + 0.2, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov, + variable_indices); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, + cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + AbsolutePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 8a31ff529..202b0739d 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -74,7 +74,7 @@ class NormalPriorPose3DTestFixture : public ::testing::Test //!< qw, qx, qy, qz }; -TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) { // Create cost function auto q = Eigen::Quaterniond::UnitRandom(); @@ -90,117 +90,4 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} - -TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) -{ - // Create cost function for a subset of residuals - // Version with y position = 0 - std::vector indices = {0, 2, 3, 4, 5}; - Eigen::Vector3d rpy {0.0, 0.0, 0.0}; - Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - - q = Eigen::Quaterniond::UnitRandom(); - full_mean << 1.0, 0.0, 1.0, q.w(), q.x(), q.y(), q.z(); - fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { - partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); - } - - const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; - - // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); - - AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); - - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} - -TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) -{ - // Create cost function for a subset of residuals - // Version with roll, pitch = 0 - std::vector indices = {0, 1, 2, 5}; - Eigen::Vector3d rpy {0.0, 0.0, 0.1}; - Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); - fuse_core::Matrix partial_sqrt_information; - - for (size_t i = 0; i < indices.size(); ++i) { - partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); - } - - const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; - - // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); - - AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); - - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} - -TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) -{ - // Create cost function for a subset of residuals - // Version with position = 0, roll = 0 - std::vector indices = {4, 5}; - Eigen::Vector3d rpy {0.0, -0.2, 0.1}; - Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - full_mean << 0.0, 0.0, 0.0, q.w(), q.x(), q.y(), q.z(); - fuse_core::Matrix partial_sqrt_information; - - for (size_t i = 0; i < indices.size(); ++i) { - partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); - } - - const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; - - // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); - - AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); - - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} - -TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) -{ - // Create cost function for a subset of residuals - // Version with z = 0, orientation = 0 - std::vector indices = {0, 1}; - Eigen::Vector3d rpy {0.0, 0.0, 0.0}; - Eigen::Quaterniond q = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); - full_mean << 0.1, 0.5, 0.0, q.w(), q.x(), q.y(), q.z(); - fuse_core::Matrix partial_sqrt_information; - - for (size_t i = 0; i < indices.size(); ++i) { - partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); - } - - const fuse_constraints::NormalPriorPose3D cost_function{partial_sqrt_information, full_mean}; - - // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); - - AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); - - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} +} \ No newline at end of file diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp new file mode 100644 index 000000000..813397ca3 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 "cost_function_gtest.hpp" +#include +#include +#include + +/** + * @brief Test fixture that initializes a full pose 3d mean and sqrt information matrix. + */ +class NormalPriorPose3DEulerTestFixture : public ::testing::Test +{ +public: + //!< The automatic differentiation cost function type for the pose 3d cost functor + using AutoDiffNormalPriorPose3DEuler = + ceres::AutoDiffCostFunction; + + /** + * @brief Constructor + */ + NormalPriorPose3DEulerTestFixture() + { + full_sqrt_information = covariance.inverse().llt().matrixU(); + } + + const fuse_core::Matrix6d covariance = + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components + Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< roll, pitch, and yaw +}; + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) +{ + // Create cost function + auto rpy = Eigen::Vector3d::Random(); + full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + const fuse_constraints::NormalPriorPose3DEuler cost_function{full_sqrt_information, full_mean}; + const auto num_residuals = full_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), + num_residuals); + + // Compare the expected, automatic differentiation, cost function and the actual one + // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function + // and the second argument is the actual cost function + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) +{ + // Create cost function for a subset of residuals + // Version with y position = 0 + std::vector indices = {0, 2, 3, 4, 5}; + auto rpy = Eigen::Vector3d::Random(); + full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) +{ + // Create cost function for a subset of residuals + // Version with roll, pitch = 0 + std::vector indices = {0, 1, 2, 5}; + Eigen::Vector3d rpy = Eigen::Vector3d::Random(); + rpy(0) = 0.0; + rpy(1) = 0.0; + full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) +{ + // Create cost function for a subset of residuals + // Version with position = 0, roll = 0 + std::vector indices = {4, 5}; + Eigen::Vector3d rpy = Eigen::Vector3d::Random(); + rpy(0) = 0.0; + full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) +{ + // Create cost function for a subset of residuals + // Version with z = 0, orientation = 0 + std::vector indices = {0, 1}; + Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); +} diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 05ba9ed25..8460b83a2 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -208,11 +209,11 @@ inline void populatePartialMeasurement( } /** - * @brief Method to create sub-measurements from full measurements and append them to existing - * partial measurements - version with covariance only + * @brief Method to create covariances of sub-measurements from covariances of full measurements and append + * them to existing partial covariances * - * @param[in] covariance_full - The full covariance matrix from which we will generate the sub- - * measurement + * @param[in] covariance_full - The full covariance matrix from which we will generate the covariances of the + * sub-measurement * @param[in] indices - The indices we want to include in the sub-measurement * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append */ @@ -423,8 +424,6 @@ inline bool processAbsolutePoseWithCovariance( return false; } } - std::cout << "Pose mean partial: \n" << pose_mean_partial << std::endl; - std::cout << "Pose covariance partial: \n" << pose_covariance_partial << std::endl; // Create an absolute pose constraint auto constraint = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( @@ -511,7 +510,7 @@ inline bool processAbsolutePose3DWithCovariance( orientation->z() = transformed_message.pose.pose.orientation.z; if (position_indices.size() == 3 && orientation_indices.size() == 3) { - // Use the full pose measurement, no need to create a partial measurement + // Full pose measurement, no need to create a partial one fuse_core::Vector7d pose_mean; pose_mean << transformed_message.pose.pose.position.x, transformed_message.pose.pose.position.y, @@ -552,35 +551,45 @@ inline bool processAbsolutePose3DWithCovariance( return true; } + /* + TODO(giafranchini): in case of partial measurements, is probably better to implement something like + AbsolutePose3DStampedEulerConstraint. This should accept: + - variable indices (std::vector) + And create a constraint for the partial measurement. The translational part of the cost function + is trivial, while the orientation part will be copied from absolute_orientation_3d_stamped_euler + + PRO: + - no need to double convert from quaternion to rpy and back + - refactor of absolute_pose_3d_stamped_constraint (and relative cost functions) with fixed size + CONS: + - a conversion from quaternion to rpy will be performed also for measurements which are partial + in position but full in orientation. On the other hand, to distinguish all the cases we would need + a lot of combinations + */ + // Convert the ROS message into tf2 transform tf2::Transform tf2_pose; tf2::fromMsg(transformed_message.pose.pose, tf2_pose); // Fill eigen pose in RPY representation - fuse_core::Vector6d pose_mean; - pose_mean.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); - tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean(3), pose_mean(4), pose_mean(5)); + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); - + // Set the components which are not measured to zero const auto indices = mergeIndices(position_indices, orientation_indices, 3); - std::replace_if( - pose_mean.data(), pose_mean.data() + pose_mean.size(), - [&indices, &pose_mean](const double & value) { - return std::find(indices.begin(), indices.end(), &value - pose_mean.data()) == indices.end(); + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&indices, &pose_mean_partial](const double & value) { + return std::find(indices.begin(), indices.end(), &value - pose_mean_partial.data()) == indices.end(); }, 0.0); - fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); - populatePartialMeasurement(pose_covariance, indices, pose_covariance_partial); - - tf2::Quaternion q_partial; - q_partial.setRPY(pose_mean(3), pose_mean(4), pose_mean(5)); + populatePartialMeasurement( + pose_covariance, + indices, + pose_covariance_partial); - // Create the pose for the constraint - fuse_core::Vector7d pose_mean_partial; - pose_mean_partial << pose_mean(0), pose_mean(1), pose_mean(2), - q_partial.getW(), q_partial.getX(), q_partial.getY(), q_partial.getZ(); - if (validate) { try { validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); @@ -592,8 +601,8 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - - auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( + + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( source, *position, *orientation, @@ -611,165 +620,6 @@ inline bool processAbsolutePose3DWithCovariance( return true; } -/** - * @brief Extracts 3D orientation data from a PoseWithCovarianceStamped message and adds that data to a - * fuse Transaction - * - * This method effectively adds a 3D orientation variable and a 3D orientation - * constraint to the given \p transaction. The orientation data is extracted from the \p pose message. - * The data will be automatically transformed into the \p target_frame before it is used. - * - * @param[in] source - The name of the sensor or motion model that generated this constraint - * @param[in] device_id - The UUID of the machine - * @param[in] pose - The PoseWithCovarianceStamped message from which we will extract the orientation data - * @param[in] loss - The loss function for the 3D pose constraint generated - * @param[in] target_frame - The frame ID into which the pose data will be transformed before it is - * used - * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction - * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform - * @param[in] validate - Whether to validate the measurements or not. If the validation fails no - * constraint is added - * @param[out] transaction - The generated variables and constraints are added to this transaction - * @return true if any constraints were added, false otherwise - */ -inline bool processAbsoluteOrientation3DWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const fuse_core::Loss::SharedPtr & loss, - const std::string & target_frame, - const std::vector & orientation_indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) -{ - if (orientation_indices.empty()) { - return false; - } - - geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; - if (target_frame.empty()) { - transformed_message = pose; - } else { - transformed_message.header.frame_id = target_frame; - - if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Failed to transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() << ". Cannot create constraint."); - return false; - } - } - // Create the orientation variable - auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); - orientation->w() = transformed_message.pose.pose.orientation.w; - orientation->x() = transformed_message.pose.pose.orientation.x; - orientation->y() = transformed_message.pose.pose.orientation.y; - orientation->z() = transformed_message.pose.pose.orientation.z; - - if (orientation_indices.size() == 3) { - // Use the full orientation measurement, no need to create a partial measurement - fuse_core::Vector4d orientation_mean; - orientation_mean << - transformed_message.pose.pose.orientation.w, - transformed_message.pose.pose.orientation.x, - transformed_message.pose.pose.orientation.y, - transformed_message.pose.pose.orientation.z; - - Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); - fuse_core::Matrix3d orientation_covariance = pose_covariance.block<3,3>(3,3); - - if (validate) { - try { - validatePartialMeasurement(orientation_mean, orientation_covariance, 1e-5); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial absolute pose measurement from '" << source - << "' source: " << ex.what()); - return false; - } - } - - auto constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - source, - *orientation, - orientation_mean, - orientation_covariance); - - constraint->loss(loss); - - transaction.addVariable(orientation); - transaction.addConstraint(constraint); - transaction.addInvolvedStamp(pose.header.stamp); - - return true; - } - - // TODO(giafranchini): in case of partial measurements, is probably better to use the - // AbsoluteOrientation3DStampedEulerConstraint, since already implements support for these. - - // Convert the ROS message into tf2 quaternion - tf2::Quaternion q; - tf2::fromMsg(transformed_message.pose.pose.orientation, q); - // Fill eigen orientation in RPY representation - fuse_core::Vector3d orientation_mean; - tf2::Matrix3x3(q).getRPY(orientation_mean(0), orientation_mean(1), orientation_mean(2)); - - Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); - fuse_core::Matrix3d orientation_covariance = pose_covariance.block<3,3>(3,3); - - // Set the components which are not measured to zero - std::replace_if( - orientation_mean.data(), orientation_mean.data() + orientation_mean.size(), - [&orientation_indices, &orientation_mean](const double & value) { - return std::find( - orientation_indices.begin(), - orientation_indices.end(), - &value - orientation_mean.data()) == orientation_indices.end(); - }, - 0.0); - - fuse_core::MatrixXd orientation_covariance_partial(orientation_indices.size(), orientation_indices.size()); - populatePartialMeasurement(orientation_covariance, orientation_indices, orientation_covariance_partial); - - tf2::Quaternion q_partial; - q_partial.setRPY(orientation_mean(0), orientation_mean(1), orientation_mean(2)); - - // Create the orientation for the constraint - fuse_core::Vector4d orientation_mean_partial; - orientation_mean_partial << q_partial.getW(), q_partial.getX(), q_partial.getY(), q_partial.getZ(); - - if (validate) { - try { - validatePartialMeasurement(orientation_mean_partial, orientation_covariance_partial, 1e-5); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial absolute pose measurement from '" << source - << "' source: " << ex.what()); - return false; - } - } - - auto constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - source, - *orientation, - orientation_mean_partial, - orientation_covariance_partial, - orientation_indices); - - constraint->loss(loss); - - transaction.addVariable(orientation); - transaction.addConstraint(constraint); - transaction.addInvolvedStamp(pose.header.stamp); - - return true; -} - /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -1274,10 +1124,6 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } - // std::cout << "Relative pose: " << std::endl; - // std::cout << " Position: " << p12.first.first.transpose() << std::endl; - // std::cout << " Rotation: " << p12.first.second << std::endl; - // std::cout << " Covariance: " << p12.second << std::endl; // Convert the poses into RPY representation covariance_geometry::PoseRPYCovariance p1_rpy, p2_rpy, p12_rpy; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( @@ -1288,6 +1134,10 @@ inline bool processDifferentialPose3DWithCovariance( p12, p12_rpy); p12_rpy.second += minimum_pose_relative_covariance; + + // TODO(giafranchini): here is probably better to create some new eigen poses + // (p1_partial, p2_partial, p12_partial) and run just one replace_if for each one of them + // Set the components which are not measured to zero // p1_partial std::replace_if( diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 54c689837..f20f5d838 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -173,12 +173,13 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) if (params_.differential) { processDifferential(*pose, validate, *transaction); } else { - common::processAbsoluteOrientation3DWithCovariance( + common::processAbsolutePose3DWithCovariance( name(), device_id_, *pose, params_.pose_loss, params_.orientation_target_frame, + {}, params_.orientation_indices, *tf_buffer_, validate, diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp index 0f88ad6c6..26e6084f1 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +// #include #include #include diff --git a/fuse_models/test/utils.py b/fuse_models/test/utils.py new file mode 100644 index 000000000..bca5644bb --- /dev/null +++ b/fuse_models/test/utils.py @@ -0,0 +1,93 @@ +import numpy as np +import quaternion +from math import sin, cos, tan + +p1x = 0.0 +p1y = 0.0 +p1z = 0.0 +r1 = -2.490 +p1 = -0.206 +y1 = 3.066 +v1x = 0.1 +v1y = 0.2 +v1z = 0.1 +w1x = 1.570796327 +w1y = 1.570796327 +w1z = -1.570796327 +a1x = -0.5 +a1y = 1.0 +a1z = 1.0 +dt = 0.1 + +def get_quaternion(roll, pitch, yaw): + w = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/ 2) * sin(pitch / 2) * sin(yaw / 2) + x = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2) + y = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2) + z = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + q = np.quaternion() + q.w = w + q.x = x + q.y = y + q.z = z + q = q.normalized() + return q + + +def main(): + p2x = p1x + \ + ((cos(y1)*cos(p1))*v1x + (cos(y1)*sin(p1)*sin(r1) - sin(y1)*cos(r1))*v1y + (cos(y1)*sin(p1)*cos(r1) + sin(y1)*sin(r1))*v1z) * dt + \ + ((cos(y1)*cos(p1))*a1x + (cos(y1)*sin(p1)*sin(r1) - sin(y1)*cos(r1))*a1y + (cos(y1)*sin(p1)*cos(r1) + sin(y1)*sin(r1))*a1z) * dt * dt * 0.5 + + p2y = p1y + \ + ((sin(y1)*cos(p1))*v1x + (sin(y1)*sin(p1)*sin(r1) + cos(y1)*cos(r1))*v1y + (sin(y1)*sin(p1)*cos(r1) - cos(y1)*sin(r1))*v1z) * dt + \ + ((sin(y1)*cos(p1))*a1x + (sin(y1)*sin(p1)*sin(r1) + cos(y1)*cos(r1))*a1y + (sin(y1)*sin(p1)*cos(r1) - cos(y1)*sin(r1))*a1z) * dt * dt * 0.5 + + p2z = p1z + \ + ((-sin(p1))*v1x + (cos(p1)*sin(r1))*v1y + (cos(p1)*cos(r1))*v1z) * dt + \ + ((-sin(p1))*a1x + (cos(p1)*sin(r1))*a1y + (cos(p1)*cos(r1))*a1z) * dt * dt * 0.5 + + r2 = r1 + \ + (w1x + (sin(r1)*tan(p1))*w1y + (cos(r1)*tan(p1))*w1z) * dt + + p2 = p1 + \ + ((cos(r1))*w1y + (-sin(r1))*w1z) * dt + + y2 = y1 + \ + ((sin(r1)/cos(p1))*w1y + (cos(r1)/cos(p1))*w1z) * dt + + q2 = get_quaternion(r2, p2, y2) + + v2x = v1x + a1x * dt + v2y = v1y + a1y * dt + v2z = v1z + a1z * dt + + w2x = w1x + w2y = w1y + w2z = w1z + + a2x = a1x + a2y = a1y + a2z = a1z + + print("p2x: ", p2x) + print("p2y: ", p2y) + print("p2z: ", p2z) + print("r2: ", r2) + print("p2: ", p2) + print("y2: ", y2) + print("q2w: ", q2.w) + print("q2x: ", q2.x) + print("q2y: ", q2.y) + print("q2z: ", q2.z) + print("v2x: ", v2x) + print("v2y: ", v2y) + print("v2z: ", v2z) + print("w2x: ", w2x) + print("w2y: ", w2y) + print("w2z: ", w2z) + print("a2x: ", a2x) + print("a2y: ", a2y) + print("a2z: ", a2z) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 3e7a91f30..424be020a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -255,6 +255,8 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); + // std::cout << summary_.FullReport() << std::endl; + // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); From 017242ae35b95a0971ce275e646503118e9bbc0f Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 2 Jan 2024 17:57:09 +0000 Subject: [PATCH 051/116] fix: typo in quat2rpy + tests --- fuse_core/include/fuse_core/util.hpp | 8 ++++---- fuse_core/test/test_util.cpp | 26 ++++++++++++++++++++++++++ 2 files changed, 30 insertions(+), 4 deletions(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index cab631e68..1908eff2a 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -170,22 +170,22 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob const double qx = q[1]; const double qy = q[2]; const double qz = q[3]; - const double discr = qz * qy - qx * qz; + const double discr = qw * qy - qx * qz; jacobian_map.setZero(); if (discr > 0.49999) { // pitch = 90 deg - jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; } else if (discr < -0.49999) { // pitch = -90 deg - jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; } else { // Non-degenerate case: - jacobian_map(0, 0) = + jacobian_map(0, 0) = -(2.0 * qx) / ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 49851fe4b..7976550f2 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -81,3 +81,29 @@ TEST(Util, wrapAngle2D) EXPECT_EQ("~b", fuse_core::joinTopicName("a/", "~b")); } } + +TEST(Util, quaternion2rpy) +{ + double q[4] = {1.0, 0.0, 0.0, 0.0}; + double rpy[3]; + fuse_core::quaternion2rpy(q, rpy); + EXPECT_EQ(0.0, rpy[0]); + EXPECT_EQ(0.0, rpy[1]); + EXPECT_EQ(0.0, rpy[2]); + + q[0] = 0.9818562; + q[1] = 0.0640713; + q[2] = 0.0911575; + q[3] = -0.1534393; + + fuse_core::quaternion2rpy(q, rpy); + EXPECT_NEAR(0.1, rpy[0], 1e-6); + EXPECT_NEAR(0.2, rpy[1], 1e-6); + EXPECT_NEAR(-0.3, rpy[2], 1e-6); + + //TODO(giafranchini): Add test for jacobian +} + +//TODO(giafranchini): Add test for quaternionProduct +//TODO(giafranchini): Add test for quaternionInverse + From 436652d1c026c685c8616d6967777f8a5ba5f146 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 2 Jan 2024 17:58:06 +0000 Subject: [PATCH 052/116] chore: cost_function_gtest support for quaternions --- .../src/normal_prior_pose_3d_euler.cpp | 7 +++--- fuse_constraints/test/cost_function_gtest.hpp | 19 ++++++++++++-- .../test/test_normal_prior_pose_3d_euler.cpp | 25 +++++++++++-------- 3 files changed, 35 insertions(+), 16 deletions(-) diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index 15e340bf0..e07415d37 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -61,20 +61,21 @@ bool NormalPriorPose3DEuler::Evaluate( double orientation_rpy[3]; double j_quat2rpy[12]; + fuse_core::quaternion2rpy(parameters[1], orientation_rpy, j_quat2rpy); + // Compute the position residual full_residuals(0) = parameters[0][0] - b_(0); full_residuals(1) = parameters[0][1] - b_(1); full_residuals(2) = parameters[0][2] - b_(2); // Compute the orientation residual - fuse_core::quaternion2rpy(parameters[1], orientation_rpy, j_quat2rpy); full_residuals(3) = orientation_rpy[0] - b_(3); full_residuals(4) = orientation_rpy[1] - b_(4); full_residuals(5) = orientation_rpy[2] - b_(5); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map = A_ * full_residuals; if (jacobians != nullptr) { @@ -87,7 +88,7 @@ bool NormalPriorPose3DEuler::Evaluate( Eigen::Map> j_quat2angle_map(j_quat2rpy); Eigen::Map(jacobians[1], num_residuals(), 4) = A_.rightCols<3>() * j_quat2angle_map; - } + } } return true; } diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 69def70b5..791c62fe4 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -38,6 +38,7 @@ #include #include +#include /** * @brief A helper function to compare a expected and actual cost function. @@ -70,8 +71,22 @@ static void ExpectCostFunctionsAreEqual( } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) { - parameters[i] = static_cast(i) + 1.0; + if ((num_parameters == 7) && (parameter_block_sizes[0] == 3) && + (parameter_block_sizes[1] == 4)) + { + // Special case for parameters[1] as quaternion + for (size_t i = 0; i < 3; i++) { + parameters[i] = static_cast(i) + 1.0; + } + Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); + parameters[3] = q.w(); + parameters[4] = q.x(); + parameters[5] = q.y(); + parameters[6] = q.z(); + } else { + for (size_t i = 0; i < num_parameters; ++i) { + parameters[i] = static_cast(i) + 1.0; + } } std::unique_ptr residuals(new double[num_residuals]); diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 813397ca3..eb6d436f4 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -104,6 +104,9 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } + std::cout << "full_mean: " << full_mean << std::endl; + std::cout << "partial_sqrt_information: " << partial_sqrt_information << std::endl; + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; // Create automatic differentiation cost function @@ -143,14 +146,13 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) { // Create cost function for a subset of residuals - // Version with position = 0, roll = 0 - std::vector indices = {4, 5}; - Eigen::Vector3d rpy = Eigen::Vector3d::Random(); - rpy(0) = 0.0; - full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); + // Version with z = 0, orientation = 0 + std::vector indices = {0, 1}; + Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; for (size_t i = 0; i < indices.size(); ++i) { @@ -169,13 +171,14 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) { // Create cost function for a subset of residuals - // Version with z = 0, orientation = 0 - std::vector indices = {0, 1}; - Eigen::Vector3d rpy {0.0, 0.0, 0.0}; - full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); + // Version with position = 0, roll = 0 + std::vector indices = {4, 5}; + Eigen::Vector3d rpy = Eigen::Vector3d::Random(); + rpy(0) = 0.0; + full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; for (size_t i = 0; i < indices.size(); ++i) { From 7f9bb01983a33c75676055fd531fb230f2160990 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 4 Jan 2024 18:26:26 +0000 Subject: [PATCH 053/116] refactor: relative_pose_3d fixed size --- .../normal_delta_pose_3d_cost_functor.hpp | 23 +++---- .../relative_pose_3d_stamped_constraint.hpp | 29 +-------- .../relative_pose_3d_stamped_constraint.cpp | 60 +------------------ 3 files changed, 15 insertions(+), 97 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index 073cfb945..1b7c41f3a 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -79,7 +79,7 @@ class NormalDeltaPose3DCostFunctor * order (dx, dy, dz, dqx, dqy, dqz) * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) */ - NormalDeltaPose3DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector7d & b); + NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values @@ -93,7 +93,7 @@ class NormalDeltaPose3DCostFunctor T * residual) const; private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix fuse_core::Vector7d b_; //!< The measured difference between variable pose1 and variable pose2 @@ -101,15 +101,13 @@ class NormalDeltaPose3DCostFunctor }; NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor( - const fuse_core::MatrixXd & A, + const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will // not be scaled { - CHECK_GT(A_.rows(), 0); - CHECK_EQ(A_.cols(), 6); } template @@ -120,8 +118,6 @@ bool NormalDeltaPose3DCostFunctor::operator()( const T * const orientation2, T * residual) const { - T full_residuals[6]; - // Compute the position delta between pose1 and pose2 T orientation1_inverse[4] = { @@ -143,18 +139,17 @@ bool NormalDeltaPose3DCostFunctor::operator()( position_delta_rotated); // Compute the first three residual terms as (position_delta - b) - full_residuals[0] = position_delta_rotated[0] - T(b_[0]); - full_residuals[1] = position_delta_rotated[1] - T(b_[1]); - full_residuals[2] = position_delta_rotated[2] - T(b_[2]); + residual[0] = position_delta_rotated[0] - T(b_[0]); + residual[1] = position_delta_rotated[1] - T(b_[1]); + residual[2] = position_delta_rotated[2] - T(b_[2]); // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation1, orientation2, &full_residuals[3]); + orientation_functor_(orientation1, orientation2, &residual[3]); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> full_residuals_map(full_residuals); - Eigen::Map> residuals_map(residual, A_.rows()); - residuals_map = A_.template cast() * full_residuals_map; + Eigen::Map> residuals_map(residual); + residuals_map.applyOnTheLeft(A_.template cast()); return true; } diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index 49ac524e6..52e4f503d 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -94,29 +94,6 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint const fuse_core::Vector7d & delta, const fuse_core::Matrix6d & covariance); - /** - * @brief Constructor (version for partial measurements) - * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] position1 The variable representing the position components of the first pose - * @param[in] orientation1 The variable representing the orientation components of the first pose - * @param[in] position2 The variable representing the position components of the second pose - * @param[in] orientation2 The variable representing the orientation components of the second pose - * @param[in] delta The measured change in the pose - * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) - * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) - * @param[in] variable_indices The indices of the measured variables - */ - RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); - /** * @brief Destructor */ @@ -130,12 +107,12 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. */ - fuse_core::MatrixXd covariance() const; + fuse_core::Matrix6d covariance() const {return (sqrt_information_.transpose() * sqrt_information_).inverse();}; /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -158,7 +135,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint protected: fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) - fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the + fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) private: diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index c76a6353f..1aefeb5fe 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -59,60 +59,6 @@ RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( { } -RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta) -{ - // Compute the partial sqrt information matrix of the provided cov matrix - fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; - - // Assemble a sqrt information matrix from the provided values, but in proper Variable order - // - // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 - // If we are measuring a subset of dimensions, we only want to produce costs for the measured - // dimensions. But the variable vectors will be full sized. We can make this all work out by - // creating a non-square A, where each row computes a cost for one measured dimensions, - // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions - sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); - for (size_t i = 0; i < variable_indices.size(); ++i) - { - sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); - } - // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - // std::cout << "mean_ = " << mean_.transpose() << std::endl; -} - -fuse_core::MatrixXd RelativePose3DStampedConstraint::covariance() const -{ - if (sqrt_information_.rows() == 6) - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } - // Otherwise we need to compute the pseudoinverse - // cov = (sqrt_info' * sqrt_info)^-1 - // With some linear algebra, we can swap the transpose and the inverse. - // cov = (sqrt_info^-1) * (sqrt_info^-1)' - // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. - // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). - // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. - auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); - fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); - return pinv * pinv.transpose(); -} - void RelativePose3DStampedConstraint::print(std::ostream & stream) const { stream << type() << "\n" @@ -128,9 +74,9 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_), - sqrt_information_.rows()); + // TODO(giafranchini): implement cost function with analytical derivatives + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); } } // namespace fuse_constraints From 60a83637787fdc731b7e723484614a18e80daae4 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 4 Jan 2024 18:28:09 +0000 Subject: [PATCH 054/116] feature: relative_pose_3d_euler constraint --- fuse_constraints/CMakeLists.txt | 1 + ...ormal_delta_pose_3d_euler_cost_functor.hpp | 161 ++++++++++++++++++ ...ative_pose_3d_stamped_euler_constraint.hpp | 47 +++-- ...ative_pose_3d_stamped_euler_constraint.cpp | 41 ++--- 4 files changed, 202 insertions(+), 48 deletions(-) create mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 0be5d3887..39a9cb225 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(${PROJECT_NAME} src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp + src/relative_pose_3d_stamped_euler_constraint.cpp src/uuid_ordering.cpp src/variable_constraints.cpp ) diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..a4eaa6826 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,161 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * 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_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D pose variables. + * + * A single pose involves two variables: a 3D position and a 3D orientation. This cost function + * computes the difference using standard 3D transformation math: + * + * cost(x) = || A * [ q1^-1 * (p2 - p1) - b(0:2) ] ||^2 + * || [ quat2rpy(q1^-1 * q2) - b(3:5) ] || + * + * where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, + * and the matrix A and the vector b are fixed. In case the user is interested in implementing a + * cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + * + */ +class NormalDeltaPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Constructor + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (dx, dy, dz, droll, dpitch, dyaw) + * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) + */ + NormalDeltaPose3DEulerCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector6d & b); + + /** + * @brief Compute the cost values/residuals using the provided variable/parameter values + */ + template + bool operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector6d b_; +}; + +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( + const fuse_core::Matrix6d & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); +} + +template +bool NormalDeltaPose3DEulerCostFunctor::operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const +{ + T full_residuals[6]; + T position_delta_rotated[3]; + T orientation_delta[4]; + T orientation_delta_rpy[3]; + + T orientation1_inverse[4] + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + + T position_delta[3] + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + + // Compute the position residual + ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); + full_residuals[0] = position_delta_rotated[0] - T(b_(0)); + full_residuals[1] = position_delta_rotated[1] - T(b_(1)); + full_residuals[2] = position_delta_rotated[2] - T(b_(2)); + + // Compute the orientation residual + ceres::QuaternionProduct(orientation1_inverse, orientation2, orientation_delta); + orientation_delta_rpy[0] = fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[1] = fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[2] = fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + full_residuals[3] = orientation_delta_rpy[0] - T(b_(3)); + full_residuals[4] = orientation_delta_rpy[1] - T(b_(4)); + full_residuals[5] = orientation_delta_rpy[2] - T(b_(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp index 5d9d90fe3..a5f2fdf9d 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,10 +31,9 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -// TODO(giafranchini): still to be implemented -#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ -#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ +#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ #include @@ -65,18 +64,18 @@ namespace fuse_constraints * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. */ -class RelativePose3DStampedConstraint : public fuse_core::Constraint +class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint { public: - FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedConstraint) + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedEulerConstraint) /** * @brief Default constructor */ - RelativePose3DStampedConstraint() = default; + RelativePose3DStampedEulerConstraint() = default; /** - * @brief Constructor + * @brief Create a constraint using a measurement/prior of the relative 3D pose * * @param[in] source The name of the sensor or motion model that generated this constraint * @param[in] position1 The variable representing the position components of the first pose @@ -84,50 +83,50 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * @param[in] position2 The variable representing the position components of the second pose * @param[in] orientation2 The variable representing the orientation components of the second pose * @param[in] delta The measured change in the pose - * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) - * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) */ - RelativePose3DStampedConstraint( + RelativePose3DStampedEulerConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, const fuse_variables::Position3DStamped & position2, const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, + const fuse_core::Vector6d & delta, const fuse_core::Matrix6d & covariance); /** - * @brief Constructor (version for partial measurements) + * @brief Create a constraint using a measurement/prior of the relative 3D pose * * @param[in] source The name of the sensor or motion model that generated this constraint * @param[in] position1 The variable representing the position components of the first pose * @param[in] orientation1 The variable representing the orientation components of the first pose * @param[in] position2 The variable representing the position components of the second pose * @param[in] orientation2 The variable representing the orientation components of the second pose - * @param[in] delta The measured change in the pose - * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) - * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] partial_delta The measured change in the pose + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) * @param[in] variable_indices The indices of the measured variables */ - RelativePose3DStampedConstraint( + RelativePose3DStampedEulerConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, const fuse_variables::Position3DStamped & position2, const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, + const fuse_core::Vector6d & partial_delta, const fuse_core::MatrixXd & partial_covariance, const std::vector & variable_indices); /** * @brief Destructor */ - virtual ~RelativePose3DStampedConstraint() = default; + virtual ~RelativePose3DStampedEulerConstraint() = default; /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector7d & delta() const {return delta_;} + const fuse_core::Vector6d & delta() const {return delta_;} /** * @brief Read-only access to the square root information matrix. @@ -137,7 +136,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Compute the measurement covariance matrix. */ - fuse_core::MatrixXd covariance() const; + fuse_core::Matrix6d covariance() const; /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -159,7 +158,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint ceres::CostFunction * costFunction() const override; protected: - fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -185,6 +184,6 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint } // namespace fuse_constraints -BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedEulerConstraint); -#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_HPP_ +#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp index 6c5912535..0c4e7eee3 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,26 +31,25 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -// TODO(giafranchini): still to be implemented #include #include #include -#include -#include +#include +#include #include namespace fuse_constraints { -RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, const fuse_variables::Position3DStamped & position2, const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, + const fuse_core::Vector6d & delta, const fuse_core::Matrix6d & covariance) : fuse_core::Constraint( source, @@ -60,23 +59,22 @@ RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( { } -RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, const fuse_variables::Position3DStamped & position2, const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, + const fuse_core::Vector6d & partial_delta, const fuse_core::MatrixXd & partial_covariance, const std::vector & variable_indices) : fuse_core::Constraint( source, {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta) + delta_(partial_delta) { // Compute the partial sqrt information matrix of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - // std::cout << "partial_sqrt_information: \n" << partial_sqrt_information << std::endl; // Assemble a sqrt information matrix from the provided values, but in proper Variable order // @@ -92,17 +90,11 @@ RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( { sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); } - // std::cout << "sqrt_information_ = " << "\n" << sqrt_information_ << std::endl; - // std::cout << "mean_ = " << mean_.transpose() << std::endl; } -fuse_core::MatrixXd RelativePose3DStampedConstraint::covariance() const +fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const { - if (sqrt_information_.rows() == 6) - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } - // Otherwise we need to compute the pseudoinverse + // We need to compute the pseudoinverse // cov = (sqrt_info' * sqrt_info)^-1 // With some linear algebra, we can swap the transpose and the inverse. // cov = (sqrt_info^-1) * (sqrt_info^-1)' @@ -114,7 +106,7 @@ fuse_core::MatrixXd RelativePose3DStampedConstraint::covariance() const return pinv * pinv.transpose(); } -void RelativePose3DStampedConstraint::print(std::ostream & stream) const +void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -127,14 +119,15 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const +ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_), + // TODO(giafranchini): implement cost function with analytical Jacobians + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), sqrt_information_.rows()); } } // namespace fuse_constraints -BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedConstraint, fuse_core::Constraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedEulerConstraint, fuse_core::Constraint); From b2058a8e7655d16dd6c768666a0e2c657aa608fa Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 4 Jan 2024 18:29:20 +0000 Subject: [PATCH 055/116] refactor: sensor_proc + models relative pose update --- .../fuse_models/common/sensor_proc.hpp | 405 +++++++++++++----- fuse_models/include/fuse_models/imu_3d.hpp | 1 + .../include/fuse_models/odometry_3d.hpp | 1 + .../fuse_models/parameters/imu_3d_params.hpp | 13 +- .../parameters/odometry_3d_params.hpp | 22 +- fuse_models/src/imu_3d.cpp | 85 ++-- fuse_models/src/odometry_3d.cpp | 57 ++- 7 files changed, 411 insertions(+), 173 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 8460b83a2..a45fb8eb3 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -551,22 +552,6 @@ inline bool processAbsolutePose3DWithCovariance( return true; } - /* - TODO(giafranchini): in case of partial measurements, is probably better to implement something like - AbsolutePose3DStampedEulerConstraint. This should accept: - - variable indices (std::vector) - And create a constraint for the partial measurement. The translational part of the cost function - is trivial, while the orientation part will be copied from absolute_orientation_3d_stamped_euler - - PRO: - - no need to double convert from quaternion to rpy and back - - refactor of absolute_pose_3d_stamped_constraint (and relative cost functions) with fixed size - CONS: - - a conversion from quaternion to rpy will be performed also for measurements which are partial - in position but full in orientation. On the other hand, to distinguish all the cases we would need - a lot of combinations - */ - // Convert the ROS message into tf2 transform tf2::Transform tf2_pose; tf2::fromMsg(transformed_message.pose.pose, tf2_pose); @@ -1070,7 +1055,7 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } else { - // TODO(giafranchini): check this method, results are nosense + // TODO(giafranchini): check this methodresults are nosense // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. // We know cov1 and cov2 and we should substract the first to the second in order // to obtain the relative pose covariance. But first the 2 of them have to be in the @@ -1124,104 +1109,86 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << + p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), + p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), + p12.first.second.z(); + fuse_core::Matrix6d pose_relative_covariance = p12.second; + + if (validate) { + try { + validateMeasurement( + pose_relative_mean, pose_relative_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + // Convert the poses into RPY representation - covariance_geometry::PoseRPYCovariance p1_rpy, p2_rpy, p12_rpy; - covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( - p1, p1_rpy); - covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( - p2, p2_rpy); + covariance_geometry::PoseRPYCovariance p12_rpy; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( p12, p12_rpy); p12_rpy.second += minimum_pose_relative_covariance; - // TODO(giafranchini): here is probably better to create some new eigen poses - // (p1_partial, p2_partial, p12_partial) and run just one replace_if for each one of them - - // Set the components which are not measured to zero - // p1_partial - std::replace_if( - p1_rpy.first.first.data(), p1_rpy.first.first.data() + p1_rpy.first.first.size(), - [&position_indices, &p1_rpy](const double & value) { - return std::find( - position_indices.begin(), - position_indices.end(), - &value - p1_rpy.first.first.data()) == position_indices.end(); - }, 0.0); + fuse_core::Vector6d pose_relative_mean_partial; + pose_relative_mean_partial << + p12_rpy.first.first.x(), p12_rpy.first.first.y(), p12_rpy.first.first.z(), + p12_rpy.first.second.x(), p12_rpy.first.second.y(), p12_rpy.first.second.z(); - std::replace_if( - p1_rpy.first.second.data(), p1_rpy.first.second.data() + p1_rpy.first.second.size(), - [&orientation_indices, &p1_rpy](const double & value) { - return std::find( - orientation_indices.begin(), - orientation_indices.end(), - &value - p1_rpy.first.second.data()) == orientation_indices.end(); - }, 0.0); - - // p2_partial - std::replace_if( - p2_rpy.first.first.data(), p2_rpy.first.first.data() + p2_rpy.first.first.size(), - [&position_indices, &p2_rpy](const double & value) { - return std::find( - position_indices.begin(), - position_indices.end(), - &value - p2_rpy.first.first.data()) == position_indices.end(); - }, 0.0); + const auto indices = mergeIndices(position_indices, orientation_indices, 3); - std::replace_if( - p2_rpy.first.second.data(), p2_rpy.first.second.data() + p2_rpy.first.second.size(), - [&orientation_indices, &p2_rpy](const double & value) { - return std::find( - orientation_indices.begin(), - orientation_indices.end(), - &value - p2_rpy.first.second.data()) == orientation_indices.end(); - }, 0.0); + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); - // p12_partial - std::replace_if( - p12_rpy.first.first.data(), p12_rpy.first.first.data() + p12_rpy.first.first.size(), - [&position_indices, &p12_rpy](const double & value) { - return std::find( - position_indices.begin(), - position_indices.end(), - &value - p12_rpy.first.first.data()) == position_indices.end(); - }, 0.0); + // Set the components which are not measured to zero std::replace_if( - p12_rpy.first.second.data(), p12_rpy.first.second.data() + p12_rpy.first.second.size(), - [&orientation_indices, &p12_rpy](const double & value) { + pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { return std::find( - orientation_indices.begin(), - orientation_indices.end(), - &value - p12_rpy.first.second.data()) == orientation_indices.end(); + indices.begin(), + indices.end(), + &value - pose_relative_mean_partial.data()) == indices.end(); }, 0.0); - // Convert back to quaternion - covariance_geometry::PoseQuaternionCovarianceRPY p1_partial, p2_partial, p12_partial; - covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( - p1_rpy, p1_partial); - covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( - p2_rpy, p2_partial); - covariance_geometry::Pose3DRPYCovarianceTo3DQuaternionCovarianceRPY( - p12_rpy, p12_partial); - - // Create mean vector and partial covariance matrix for the constraint - fuse_core::Vector7d pose_relative_mean; - pose_relative_mean << - p12_partial.first.first.x(), p12_partial.first.first.y(), p12_partial.first.first.z(), - p12_partial.first.second.w(), p12_partial.first.second.x(), p12_partial.first.second.y(), - p12_partial.first.second.z(); - - const auto indices = mergeIndices(position_indices, orientation_indices, 3); - fuse_core::MatrixXd pose_relative_covariance(indices.size(), indices.size()); populatePartialMeasurement( - p12_partial.second, + p12_rpy.second, indices, - pose_relative_covariance); + pose_relative_covariance_partial); if (validate) { try { validateMeasurement( - pose_relative_mean, pose_relative_covariance, 1e-5); + pose_relative_mean_partial, pose_relative_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -1231,14 +1198,14 @@ inline bool processDifferentialPose3DWithCovariance( } } // Create a relative pose constraint. - auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( source, *position1, *orientation1, *position2, *orientation2, - pose_relative_mean, - pose_relative_covariance, + pose_relative_mean_partial, + pose_relative_covariance_partial, indices); constraint->loss(loss); @@ -1451,6 +1418,246 @@ inline bool processDifferentialPoseWithTwistCovariance( return true; } +/** + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction + * + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. 3D data is used. The pose delta + * is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the twist covariance of the last message is used to compute the relative pose + * covariance using the time difference between the pose_absolute2 and pose_absolute1 time stamps. + * This assumes the pose measurements are dependent. A small minimum relative covariance is added to + * avoid getting a zero or ill-conditioned covariance. This could happen if the twist covariance is + * very small, e.g. when the twist is zero. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] twist - The second (and temporally later) TwistWithCovarianceStamped message + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always + * added to the resulting pose relative covariance + * @param[in] twist_covariance_offset - The twist covariance offset that was added to the twist + * covariance and must be substracted from it before computing + * the pose relative covariance from it + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processDifferentialPose3DWithTwistCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const fuse_core::Matrix6d & minimum_pose_relative_covariance, + const fuse_core::Matrix6d & twist_covariance_offset, + const fuse_core::Loss::SharedPtr & loss, + const std::vector & position_indices, + const std::vector & orientation_indices, + const bool validate, + fuse_core::Transaction & transaction) +{ + // TODO(giafranchini): still to be implemented + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + // Convert the poses into tf2 transforms + + tf2::Transform pose1_tf2, pose2_tf2; + tf2::fromMsg(pose1.pose.pose, pose1_tf2); + tf2::fromMsg(pose2.pose.pose, pose2_tf2); + + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1_tf2.getOrigin().x(); + position1->y() = pose1_tf2.getOrigin().y(); + position1->z() = pose1_tf2.getOrigin().z(); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + orientation1->x() = pose1_tf2.getRotation().x(); + orientation1->y() = pose1_tf2.getRotation().y(); + orientation1->z() = pose1_tf2.getRotation().z(); + orientation1->w() = pose1_tf2.getRotation().w(); + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2_tf2.getOrigin().x(); + position2->y() = pose2_tf2.getOrigin().y(); + position2->z() = pose2_tf2.getOrigin().z(); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + orientation2->x() = pose2_tf2.getRotation().x(); + orientation2->y() = pose2_tf2.getRotation().y(); + orientation2->z() = pose2_tf2.getRotation().z(); + orientation2->w() = pose2_tf2.getRotation().w(); + + // Create the delta for the constraint + const auto delta = pose1_tf2.inverseTimes(pose2_tf2); + + // Create the covariance components for the constraint + Eigen::Map cov(twist.twist.covariance.data()); + + // For dependent pose measurements p1 and p2, we assume they're computed as: + // + // p2 = p1 * p12 [1] + // + // where p12 is the relative pose between p1 and p2, which is computed here as: + // + // p12 = p1^-1 * p2 + // + // Note that the twist t12 is computed as: + // + // t12 = p12 / dt + // + // where dt = t2 - t1, for t1 and t2 being the p1 and p2 timestamps, respectively. + // + // Therefore, the relative pose p12 is computed as follows given the twist t12: + // + // p12 = t12 * dt + // + // The covariance propagation of this equation is: + // + // C12 = J_t12 * T12 * J_t12^T [2] + // + // where T12 is the twist covariance and J_t12 is the jacobian of the equation wrt to t12. + // + // The jacobian wrt t12 is: + // + // J_t12 = dt * Id + // + // where Id is a 3x3 Identity matrix. + // + // In some cases the twist covariance T12 is very small and it could yield to an ill-conditioned + // C12 covariance. For that reason a minimum covariance is added to [2]. + // + // It is also common that for the same reason, the twist covariance T12 already has a minimum + // covariance offset added to it by the publisher, so we have to remove it before using it. + + const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); + + if (dt < 1e-6) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Very small time difference " << dt << "s from '" << source << "' source."); + return false; + } + + fuse_core::Matrix6d j_twist; + j_twist.setIdentity(); + j_twist *= dt; + + fuse_core::Matrix6d pose_relative_covariance = + j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + + minimum_pose_relative_covariance; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), + delta.getRotation().w(), delta.getRotation().x(), delta.getRotation().y(), delta.getRotation().z(); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean, pose_relative_covariance, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + + // TODO(giafranchini): implement partial pose measurement + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_relative_mean_partial; + pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(); + tf2::Matrix3x3(delta.getRotation()).getRPY( + pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { + return std::find(indices.begin(), indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); + }, 0.0); + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement( + pose_relative_covariance, + indices, + pose_relative_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean_partial, pose_relative_covariance_partial, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean_partial, + pose_relative_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + /** * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse * Transaction diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 2480f466e..5095916a6 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -160,6 +160,7 @@ class Imu3D : public fuse_core::AsyncSensorModel */ void processDifferential( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, fuse_core::Transaction & transaction); diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 26ffbbd7d..6f932595d 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -151,6 +151,7 @@ class Odometry3D : public fuse_core::AsyncSensorModel */ void processDifferential( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, fuse_core::Transaction & transaction); diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index e388fe225..9e9fff13c 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -130,13 +130,12 @@ struct Imu3DParams : public ParameterBase ns, "independent"), independent); - // TODO(giafranchini): understand if this is needed - // use_twist_covariance = - // fuse_core::getParam( - // interfaces, fuse_core::joinParameterName( - // ns, - // "use_twist_covariance"), - // use_twist_covariance); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 0ce0aac16..647846484 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -149,21 +149,21 @@ struct Odometry3DParams : public ParameterBase ns, "independent"), independent); - // use_twist_covariance = - // fuse_core::getParam( - // interfaces, fuse_core::joinParameterName( - // ns, - // "use_twist_covariance"), - // use_twist_covariance); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - // twist_covariance_offset = - // fuse_core::getCovarianceDiagonalParam<3>( - // interfaces, - // fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } pose_loss = @@ -186,7 +186,7 @@ struct Odometry3DParams : public ParameterBase bool use_twist_covariance {true}; fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - // fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance //!< matrix, that will be substracted in order to //!< recover the raw values int queue_size {10}; diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index f20f5d838..4f60f0890 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -171,7 +171,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) const bool validate = !params_.disable_checks; if (params_.differential) { - processDifferential(*pose, validate, *transaction); + processDifferential(*pose, twist, validate, *transaction); } else { common::processAbsolutePose3DWithCovariance( name(), @@ -248,6 +248,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) void Imu3D::processDifferential( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, fuse_core::Transaction & transaction) { @@ -271,47 +272,47 @@ void Imu3D::processDifferential( return; } - // if (params_.use_twist_covariance) { - // geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; - // transformed_twist.header.frame_id = - // params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - // if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - // RCLCPP_WARN_STREAM_THROTTLE( - // logger_, *clock_, 5.0 * 1000, - // "Cannot transform twist message with stamp " << rclcpp::Time( - // twist.header.stamp).nanoseconds() - // << " to twist target frame " << - // params_.twist_target_frame); - // } else { - // common::processDifferentialPoseWithTwistCovariance( - // name(), - // device_id_, - // *previous_pose_, - // *transformed_pose, - // twist, - // params_.minimum_pose_relative_covariance, - // params_.twist_covariance_offset, - // params_.pose_loss, - // {}, - // params_.orientation_indices, - // validate, - // transaction); - // } - // } else { - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); - // } + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } previous_pose_ = std::move(transformed_pose); } diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 3603e2774..ca604a7ed 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -151,7 +151,7 @@ void Odometry3D::process(const nav_msgs::msg::Odometry & msg) const bool validate = !params_.disable_checks; if (params_.differential) { - processDifferential(*pose, validate, *transaction); + processDifferential(*pose, twist, validate, *transaction); } else { common::processAbsolutePose3DWithCovariance( name(), @@ -188,6 +188,7 @@ void Odometry3D::process(const nav_msgs::msg::Odometry & msg) void Odometry3D::processDifferential( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, fuse_core::Transaction & transaction) { @@ -209,19 +210,47 @@ void Odometry3D::processDifferential( return; } - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); - + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + transformed_twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } previous_pose_ = std::move(transformed_pose); } From 7191974ba9d43a76002ba62627f2d4ac8020e905 Mon Sep 17 00:00:00 2001 From: giacomo Date: Fri, 5 Jan 2024 13:23:00 +0100 Subject: [PATCH 056/116] Fix: wrong fixed size sqrt_info in relatie euler cost functor + test --- ...ormal_delta_pose_3d_euler_cost_functor.hpp | 4 +- fuse_constraints/test/CMakeLists.txt | 1 + ...olute_pose_3d_stamped_euler_constraint.cpp | 2 +- ...ative_pose_3d_stamped_euler_constraint.cpp | 698 ++++++++++++++++++ 4 files changed, 702 insertions(+), 3 deletions(-) create mode 100644 fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index a4eaa6826..57618fd96 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -76,7 +76,7 @@ class NormalDeltaPose3DEulerCostFunctor * order (dx, dy, dz, droll, dpitch, dyaw) * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) */ - NormalDeltaPose3DEulerCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector6d & b); + NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values @@ -95,7 +95,7 @@ class NormalDeltaPose3DEulerCostFunctor }; NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( - const fuse_core::Matrix6d & A, + const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) : A_(A), b_(b) diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index c60ecff64..119c78ef6 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -15,6 +15,7 @@ set(TEST_TARGETS test_relative_constraint test_relative_pose_2d_stamped_constraint test_relative_pose_3d_stamped_constraint + test_relative_pose_3d_stamped_euler_constraint test_uuid_ordering test_variable_constraints ) diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index d994f296d..a91f79b25 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -325,7 +325,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) auto position_variable = Position3DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; - position_variable->y() = 0.0; + position_variable->y() = 1.0; position_variable->z() = 10.0; auto orientation_variable = Orientation3DStamped::make_shared( diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..4fdacf208 --- /dev/null +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,698 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * 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 +#include + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; +using fuse_constraints::RelativePose3DStampedEulerConstraint; + + +TEST(RelativePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov)); +} + +TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov, indices)); +} + +TEST(RelativePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov, + indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT + 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT + 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT + /* *INDENT-ON* */ + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov; + + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); +} + +TEST(RelativePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), + orientation1->localParameterization()); + problem.AddParameterBlock( + position1->data(), + position1->size(), + position1->localParameterization()); + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), + orientation2->localParameterization()); + problem.AddParameterBlock( + position2->data(), + position2->size(), + position2->localParameterization()); + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.0, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + std::vector indices {0, 1, 3, 4, 5}; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta = fuse_core::Matrix5d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta, + indices); + + // Create a relative pose constraint for 1m in the y direction + std::vector indices_y {1, 2, 3, 4, 5}; + fuse_core::Vector6d mean_delta_y; + mean_delta_y << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta_y = fuse_core::Matrix5d::Identity(); + auto relative_y = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta_y, + cov_delta_y, + indices_y); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), + orientation1->localParameterization()); + problem.AddParameterBlock( + position1->data(), + position1->size(), + position1->localParameterization()); + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), + orientation2->localParameterization()); + problem.AddParameterBlock( + position2->data(), + position2->size(), + position2->localParameterization()); + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + std::vector relative_parameter_blocks_y; + relative_parameter_blocks_y.push_back(position1->data()); + relative_parameter_blocks_y.push_back(orientation1->data()); + relative_parameter_blocks_y.push_back(position2->data()); + relative_parameter_blocks_y.push_back(orientation2->data()); + problem.AddResidualBlock( + relative_y->costFunction(), + relative_y->lossFunction(), + relative_parameter_blocks_y); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.5, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, + -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.25, 0.5, -1.0, 0.0, + 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, + -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, + delta, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + RelativePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.delta(), actual.delta()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} From 3d78b1a43f5fb6819167e492e88025b7ce89462f Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 8 Jan 2024 15:04:20 +0100 Subject: [PATCH 057/116] sensor_proc_3d tests --- .../fuse_models/common/sensor_proc.hpp | 34 ++-- fuse_models/test/CMakeLists.txt | 3 + fuse_models/test/test_sensor_proc.cpp | 1 - fuse_models/test/test_sensor_proc_3d.cpp | 150 ++++++++++++++++++ 4 files changed, 168 insertions(+), 20 deletions(-) create mode 100644 fuse_models/test/test_sensor_proc_3d.cpp diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index a45fb8eb3..160ac1258 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -223,7 +223,6 @@ inline void populatePartialMeasurement( const std::vector & indices, fuse_core::MatrixXd & covariance_partial) { - covariance_partial.setZero(); for (size_t r = 0; r < indices.size(); ++r) { for (size_t c = 0; c < indices.size(); ++c) { covariance_partial(r, c) = covariance_full(indices[r], indices[c]); @@ -989,10 +988,10 @@ inline bool processDifferentialPoseWithCovariance( * @param[in] device_id - The UUID of the machine * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message - * @param[in] independent - Whether the pose measurements are indepent or not + * @param[in] independent - Whether the pose measurements are independent or not * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always * added to the resulting pose relative covariance - * @param[in] loss - The loss function for the 2D pose constraint generated + * @param[in] loss - The loss function for the 3D pose constraint generated * @param[in] position_indices - The indices of the position variables to be added to the transaction * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction * @param[in] validate - Whether to validate the measurements or not. If the validation fails no @@ -1014,17 +1013,17 @@ inline bool processDifferentialPose3DWithCovariance( fuse_core::Transaction & transaction) { // TODO(giafranchini): we should probably remove covariance_geometry dependency - - // Convert from ROS msg to covariance geometry types // PoseQuaternionCovarianceRPY is std::pair, Covariance> // Position is Eigen::Vector3d // Quaternion is Eigen::Quaterniond // Covariance is Eigen::Matrix6d + + // Convert from ROS msg to covariance geometry types covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; covariance_geometry::fromROS(pose1.pose, p1); covariance_geometry::fromROS(pose2.pose, p2); -// Create the pose variables + // Create the pose variables auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); @@ -1055,7 +1054,7 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } else { - // TODO(giafranchini): check this methodresults are nosense + // TODO(giafranchini): check this method, results are nosense // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. // We know cov1 and cov2 and we should substract the first to the second in order // to obtain the relative pose covariance. But first the 2 of them have to be in the @@ -1117,7 +1116,7 @@ inline bool processDifferentialPose3DWithCovariance( p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), p12.first.second.z(); - fuse_core::Matrix6d pose_relative_covariance = p12.second; + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; if (validate) { try { @@ -1155,16 +1154,14 @@ inline bool processDifferentialPose3DWithCovariance( } // Convert the poses into RPY representation - covariance_geometry::PoseRPYCovariance p12_rpy; - covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DRPYCovariance( - p12, p12_rpy); - - p12_rpy.second += minimum_pose_relative_covariance; - fuse_core::Vector6d pose_relative_mean_partial; - pose_relative_mean_partial << - p12_rpy.first.first.x(), p12_rpy.first.first.y(), p12_rpy.first.first.z(), - p12_rpy.first.second.x(), p12_rpy.first.second.y(), p12_rpy.first.second.z(); + fuse_core::Matrix6d pose_relative_covariance = p12.second; + + tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), p12.first.second.w()); + pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), p12.first.first.z(); + tf2::Matrix3x3(q12).getRPY(pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + + pose_relative_covariance += minimum_pose_relative_covariance; const auto indices = mergeIndices(position_indices, orientation_indices, 3); @@ -1181,7 +1178,7 @@ inline bool processDifferentialPose3DWithCovariance( }, 0.0); populatePartialMeasurement( - p12_rpy.second, + pose_relative_covariance, indices, pose_relative_covariance_partial); @@ -1600,7 +1597,6 @@ inline bool processDifferentialPose3DWithTwistCovariance( return true; } - // TODO(giafranchini): implement partial pose measurement // Fill eigen pose in RPY representation fuse_core::Vector6d pose_relative_mean_partial; pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(); diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index 1cbb64482..15e15aad1 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -17,3 +17,6 @@ endforeach() ament_add_gmock(test_sensor_proc "test_sensor_proc.cpp") target_link_libraries(test_sensor_proc ${PROJECT_NAME}) + +ament_add_gmock(test_sensor_proc_3d "test_sensor_proc_3d.cpp") +target_link_libraries(test_sensor_proc_3d ${PROJECT_NAME}) \ No newline at end of file diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 7377f6626..921d67c54 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -11,7 +11,6 @@ #include namespace fm_common = fuse_models::common; -//TODO(giafranchini): Add tests for 3d sensors TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) { const std::vector position_indices{0, 1}; diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp new file mode 100644 index 000000000..29e39ff2c --- /dev/null +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -0,0 +1,150 @@ +/*************************************************************************** + * Copyright (C) 2024 Giacomo Franchini. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include +#include + +#include + +#include + +namespace fm_common = fuse_models::common; +TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) +{ + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{0, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size(), merged_indices.size()); + EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); +} + +TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(orientation_indices.size(), merged_indices.size()); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_TRUE(merged_indices.empty()); +} + +TEST(TestSuite, populatePartialMeasurements) +{ + // Test both conversion from quaternion to RPY and partial measurement population + // This one is just to generate a random unit quaternion and have the reference in RPY + fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); + fuse_core::Quaternion q = + Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + + tf2::Transform tf2_pose; + tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); + tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); + + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); + + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&merged_indices, &pose_mean_partial](const double & value) { + return std::find( + merged_indices.begin(), + merged_indices.end(), + &value - pose_mean_partial.data()) == merged_indices.end(); + }, 0.0); + + fuse_core::MatrixXd pose_covariance_partial(3, 3); + + fm_common::populatePartialMeasurement( + pose_covariance, + merged_indices, + pose_covariance_partial); + + fuse_core::Vector6d expected_pose; + expected_pose << 1.0, 2.0, 0.0, 0.0, 0.0, rpy(2); + fuse_core::Matrix3d expected_covariance; + expected_covariance.col(0) << pose_covariance(0, 0), pose_covariance(1, 0), pose_covariance(5, 0); + expected_covariance.col(1) << pose_covariance(0, 1), pose_covariance(1, 1), pose_covariance(5, 1); + expected_covariance.col(2) << pose_covariance(0, 5), pose_covariance(1, 5), pose_covariance(5, 5); + EXPECT_TRUE(expected_pose.isApprox(pose_mean_partial)); + EXPECT_TRUE(expected_covariance.isApprox(pose_covariance_partial)); +} From d710b35f1048db7a640eab076973fb75ed17487d Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 10 Jan 2024 09:14:33 +0000 Subject: [PATCH 058/116] fix: wrong expected value in test absolutePose3DStampedEulerConstraint --- .../test/test_absolute_pose_3d_stamped_euler_constraint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index a91f79b25..b5ebb2110 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -387,7 +387,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) // Check EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); - EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); From 22190e81b626072d7c0614e234c3cab8f4a7ef9c Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 10 Jan 2024 09:15:10 +0000 Subject: [PATCH 059/116] Fix: imu3D getPositiveParam account for params namespace --- .../fuse_models/parameters/imu_3d_params.hpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 9e9fff13c..9e08c7991 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -103,9 +103,16 @@ struct Imu3DParams : public ParameterBase ns, "queue_size"), queue_size); - fuse_core::getPositiveParam(interfaces, "tf_timeout", tf_timeout, false); - - fuse_core::getPositiveParam(interfaces, "throttle_period", throttle_period, false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); throttle_use_wall_time = fuse_core::getParam( interfaces, fuse_core::joinParameterName( From ff3ecffba415ec6ad685219de71cf2c7fb54408a Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 10 Jan 2024 09:16:12 +0000 Subject: [PATCH 060/116] fix tf2 conversion --- fuse_models/src/odometry_3d_publisher.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 179159884..12ee64062 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -561,8 +561,7 @@ void Odometry3DPublisher::publishTimerCallback() velocity_linear, velocity_angular, params_.velocity_linear_norm_min_, - params_.velocity_angular_norm_min_ - ); + params_.velocity_angular_norm_min_); } covariance.noalias() += dt * process_noise_covariance; @@ -589,9 +588,7 @@ void Odometry3DPublisher::publishTimerCallback() trans.header.stamp = odom_output.header.stamp; trans.header.frame_id = frame_id; trans.child_frame_id = child_frame_id; - trans.transform.translation = tf2::toMsg(pose.getOrigin()); - trans.transform.rotation = tf2::toMsg(pose.getRotation()); - + trans.transform = tf2::toMsg(pose); if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { try { auto base_to_odom = tf_buffer_->lookupTransform( From 5a33b9f4df3351d05969d83d133db28838d11c30 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 10 Jan 2024 09:24:04 +0000 Subject: [PATCH 061/116] Removed completed TODOs --- .../fuse_models/common/sensor_proc.hpp | 40 ------------------- 1 file changed, 40 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 160ac1258..1d614c224 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -1012,7 +1012,6 @@ inline bool processDifferentialPose3DWithCovariance( const bool validate, fuse_core::Transaction & transaction) { - // TODO(giafranchini): we should probably remove covariance_geometry dependency // PoseQuaternionCovarianceRPY is std::pair, Covariance> // Position is Eigen::Vector3d // Quaternion is Eigen::Quaterniond @@ -1054,7 +1053,6 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } else { - // TODO(giafranchini): check this method, results are nosense // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. // We know cov1 and cov2 and we should substract the first to the second in order // to obtain the relative pose covariance. But first the 2 of them have to be in the @@ -1461,13 +1459,11 @@ inline bool processDifferentialPose3DWithTwistCovariance( const bool validate, fuse_core::Transaction & transaction) { - // TODO(giafranchini): still to be implemented if (position_indices.empty() && orientation_indices.empty()) { return false; } // Convert the poses into tf2 transforms - tf2::Transform pose1_tf2, pose2_tf2; tf2::fromMsg(pose1.pose.pose, pose1_tf2); tf2::fromMsg(pose2.pose.pose, pose2_tf2); @@ -1501,42 +1497,6 @@ inline bool processDifferentialPose3DWithTwistCovariance( // Create the covariance components for the constraint Eigen::Map cov(twist.twist.covariance.data()); - // For dependent pose measurements p1 and p2, we assume they're computed as: - // - // p2 = p1 * p12 [1] - // - // where p12 is the relative pose between p1 and p2, which is computed here as: - // - // p12 = p1^-1 * p2 - // - // Note that the twist t12 is computed as: - // - // t12 = p12 / dt - // - // where dt = t2 - t1, for t1 and t2 being the p1 and p2 timestamps, respectively. - // - // Therefore, the relative pose p12 is computed as follows given the twist t12: - // - // p12 = t12 * dt - // - // The covariance propagation of this equation is: - // - // C12 = J_t12 * T12 * J_t12^T [2] - // - // where T12 is the twist covariance and J_t12 is the jacobian of the equation wrt to t12. - // - // The jacobian wrt t12 is: - // - // J_t12 = dt * Id - // - // where Id is a 3x3 Identity matrix. - // - // In some cases the twist covariance T12 is very small and it could yield to an ill-conditioned - // C12 covariance. For that reason a minimum covariance is added to [2]. - // - // It is also common that for the same reason, the twist covariance T12 already has a minimum - // covariance offset added to it by the publisher, so we have to remove it before using it. - const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); if (dt < 1e-6) { From a7abf32188f1bd37a1564440e2db49625ee8c72c Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 16:28:04 +0000 Subject: [PATCH 062/116] removed covariance_geometry dep from fuse_constraints --- fuse_constraints/CMakeLists.txt | 3 --- fuse_constraints/package.xml | 2 -- 2 files changed, 5 deletions(-) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 39a9cb225..f6eb3a5d3 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -12,7 +12,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) -find_package(covariance_geometry_ros REQUIRED) find_package(fuse_core REQUIRED) find_package(fuse_graphs REQUIRED) find_package(fuse_variables REQUIRED) @@ -64,7 +63,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PUBLIC Boost::serialization - covariance_geometry_ros::covariance_geometry_ros Ceres::ceres Eigen3::Eigen fuse_core::fuse_core @@ -117,7 +115,6 @@ 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 - covariance_geometry_ros fuse_core fuse_graphs fuse_variables diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index 52a07a604..ed2bc9dc7 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -20,7 +20,6 @@ libceres-dev suitesparse - covariance_geometry_ros fuse_core fuse_graphs fuse_variables @@ -28,7 +27,6 @@ pluginlib rclcpp - covariance_geometry_ros fuse_core fuse_graphs fuse_variables From 3a8a14c832e9016bf6be3cf1a4d6db1c5b467f96 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 16:45:42 +0000 Subject: [PATCH 063/116] add missing absolute constraints and cleanup --- fuse_constraints/fuse_plugins.xml | 5 +++++ .../include/fuse_constraints/absolute_constraint.hpp | 5 ++--- .../fuse_constraints/absolute_constraint_impl.hpp | 12 ++++++++++++ 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index a53a5762b..71e7b8d9a 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -148,4 +148,9 @@ A constraint that represents a measurement on the difference between two 3D poses. + + + A constraint that represents a measurement on the difference between two 3D poses, orientation parametrized in RPY. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index a0189a9c2..b9e38f4d7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -50,7 +50,6 @@ #include #include #include -// #include #include #include #include @@ -215,10 +214,10 @@ using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityAngular3DStampedConstraint = AbsoluteConstraint; -using AbsoluteVelocityLinear3DStampedConstraint = - AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = + AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index e027efb48..197db9c5e 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -155,6 +155,18 @@ inline std::string AbsoluteConstraint +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint"; +} + template<> inline std::string AbsoluteConstraint::type() const { From 901432025281098a8f275658bbe65c1508f5c06e Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 16:46:09 +0000 Subject: [PATCH 064/116] absolute orientation 3d cleanup --- ...lute_orientation_3d_stamped_constraint.hpp | 20 +------ ...lute_orientation_3d_stamped_constraint.cpp | 59 ++----------------- 2 files changed, 6 insertions(+), 73 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp index 5e025208c..0f63bdf86 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp @@ -85,22 +85,6 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector4d & mean, const fuse_core::Matrix3d & covariance); - - /** - * @brief Create a constraint using a measurement/prior of a 3D orientation - * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] orientation The variable representing the orientation components of the pose - * @param[in] mean The measured/prior orientation as a quaternion (4x1 vector: w, x, y, z) - * @param[in] partial_covariance The measurement/prior covariance (max 3x3 matrix: qx, qy, qz) - * @param[in] orientation_indices The indices of the measured variables - */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & orientation_indices); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -147,7 +131,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z) */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} /** * @brief Compute the measurement covariance matrix. @@ -198,7 +182,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint static fuse_core::Matrix3d toEigen(const std::array & covariance); fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable - fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix private: // Allow Boost Serialization access to private methods diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index e4cb8ede2..0131bfd20 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -38,7 +38,6 @@ #include #include -// #include #include #include @@ -56,34 +55,6 @@ AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( { } -AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & orientation_indices) -: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) - mean_(mean) -{ - // Compute the partial sqrt information matrix of the provided cov matrix - fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); - - // Assemble a sqrt information matrix from the provided values, but in proper Variable order - // - // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 - // If we are measuring a subset of dimensions, we only want to produce costs for the measured - // dimensions. But the variable vectors will be full sized. We can make this all work out by - // creating a non-square A, where each row computes a cost for one measured dimensions, - // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions - sqrt_information_ = fuse_core::MatrixXd::Zero(orientation_indices.size(), 3); - for (size_t i = 0; i < orientation_indices.size(); ++i) - { - sqrt_information_.col(orientation_indices[i]) = partial_sqrt_information.col(i); - } -} - AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( const std::string & source, const fuse_variables::Orientation3DStamped & orientation, @@ -104,20 +75,8 @@ AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::covariance() const { - if (sqrt_information_.rows() == 3) - { - return (sqrt_information_.transpose() * sqrt_information_).inverse(); - } - // Otherwise we need to compute the pseudoinverse - // cov = (sqrt_info' * sqrt_info)^-1 - // With some linear algebra, we can swap the transpose and the inverse. - // cov = (sqrt_info^-1) * (sqrt_info^-1)' - // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. - // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). - // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. - auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); - fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); - return pinv * pinv.transpose();} + return (sqrt_information_.transpose() * sqrt_information_).inverse(); +} void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const { @@ -136,18 +95,8 @@ void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsoluteOrientation3DStampedConstraint::costFunction() const { - // return new NormalPriorOrientation3D(sqrt_information_, mean_); - - // Here we return a cost function that computes the analytic derivatives/jacobians, but we could - // use automatic differentiation as follows: - - return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_), - sqrt_information_.rows()); - - // And including the followings: - // #include - // #include + return new ceres::AutoDiffCostFunction( + new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); } fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( From c50296aec6092209ba6a637f5fa1e57a680509fb Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 16:47:37 +0000 Subject: [PATCH 065/116] absolute pose 3d & 3d euler cleanup --- .../absolute_pose_3d_stamped_constraint.hpp | 7 +++++-- .../absolute_pose_3d_stamped_euler_constraint.hpp | 5 +---- .../src/absolute_pose_3d_stamped_constraint.cpp | 7 ------- .../src/absolute_pose_3d_stamped_euler_constraint.cpp | 4 ---- 4 files changed, 6 insertions(+), 17 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 72fd9d016..51238c8b7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -94,7 +94,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector7d & mean, const fuse_core::Matrix6d & covariance); - + /** * @brief Destructor */ @@ -119,7 +119,10 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qx, qy, qz) */ - fuse_core::Matrix6d covariance() const; + fuse_core::Matrix6d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } /** * @brief Print a human-readable description of the constraint to the provided stream. diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index 41b88a402..63c2d8502 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -71,8 +70,6 @@ namespace fuse_constraints * measurements). This constraint holds the measured 3D pose: orientation is parametrized as roll-pitch-yaw * Euler angles and the covariance represents the error around each translational and rotational axis. * This constraint allows measurement of a subset of the pose components given in the variable. - * - * (Use AbsolutePose3DStampedConstraint for full pose measurements). */ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint @@ -103,7 +100,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint const fuse_core::Matrix6d & covariance); /** - * @brief Create a constraint using a measurement/prior of the 3D pose + * @brief Create a constraint using a partial measurement/prior of the 3D pose * * @param[in] source The name of the sensor or motion model that generated this constraint * @param[in] position The variable representing the position components of the pose diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index e2454f3ad..ca248c9f0 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -39,8 +39,6 @@ #include #include #include -// #include -// #include #include namespace fuse_constraints @@ -58,11 +56,6 @@ AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( { } -fuse_core::Matrix6d AbsolutePose3DStampedConstraint::covariance() const -{ - return (sqrt_information_.transpose() * sqrt_information_).inverse(); -} - void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const { stream << type() << "\n" diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index fbe20b11a..56206e301 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -37,11 +36,9 @@ #include #include -// #include #include #include -// #include #include namespace fuse_constraints @@ -106,7 +103,6 @@ fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const return pinv * pinv.transpose(); } - void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const { stream << type() << "\n" From 727afd11d19e2852b9696c0be285f23938c6cc14 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 16:54:47 +0000 Subject: [PATCH 066/116] delta pose/orientation cost functor cleanup --- .../normal_delta_orientation_3d_cost_functor.hpp | 2 +- .../fuse_constraints/normal_delta_pose_3d_cost_functor.hpp | 7 +++---- .../normal_delta_pose_3d_euler_cost_functor.hpp | 4 ++-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index 988c623bb..a83e4479a 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -120,7 +120,7 @@ class NormalDeltaOrientation3DCostFunctor // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map> residuals_map(residuals); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map.applyOnTheLeft(A_.template cast()); return true; diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index 1b7c41f3a..41c31a497 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -146,10 +146,9 @@ bool NormalDeltaPose3DCostFunctor::operator()( // Use the 3D orientation cost functor to compute the orientation delta orientation_functor_(orientation1, orientation2, &residual[3]); - // Scale the residuals by the square root information matrix to account for - // the measurement uncertainty. - Eigen::Map> residuals_map(residual); - residuals_map.applyOnTheLeft(A_.template cast()); + // Map it to Eigen, and weight it + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); return true; } diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index 57618fd96..afb16217d 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -51,7 +51,7 @@ namespace fuse_constraints * A single pose involves two variables: a 3D position and a 3D orientation. This cost function * computes the difference using standard 3D transformation math: * - * cost(x) = || A * [ q1^-1 * (p2 - p1) - b(0:2) ] ||^2 + * cost(x) = || A * [ (q1^-1 * (p2 - p1)) - b(0:2) ] ||^2 * || [ quat2rpy(q1^-1 * q2) - b(3:5) ] || * * where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, @@ -158,4 +158,4 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( } // namespace fuse_constraints -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ \ No newline at end of file From 656d306be5b0db6fa8186699071a5c202bbadb93 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Thu, 11 Jan 2024 17:55:25 +0100 Subject: [PATCH 067/116] Update normal_delta_pose_3d_cost_functor.hpp --- .../fuse_constraints/normal_delta_pose_3d_cost_functor.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index 41c31a497..ace413e51 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -35,7 +35,6 @@ #define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_HPP_ #include -#include #include #include From 339952f88fb366f266529f9622c2818d71d2bca6 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:02:56 +0000 Subject: [PATCH 068/116] normal prior orientation 3d cleanup --- .../normal_prior_orientation_3d.hpp | 14 ++++---------- .../src/normal_prior_orientation_3d.cpp | 18 +++++------------- 2 files changed, 9 insertions(+), 23 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp index 3be895a8d..4e6c4b60e 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -64,22 +63,17 @@ namespace fuse_constraints * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the * square root information matrix (the inverse of the covariance). */ -class NormalPriorOrientation3D : public ceres::SizedCostFunction +class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> { public: /** * @brief Construct a cost function instance * - * The residual weighting matrix can vary in size, as this cost functor can be used to compute - * costs for partial vectors. The number of rows of A will be the number of dimensions for which - * you want to compute the error, and the number of columns in A will be fixed at 3. For example, - * if we just want to use the values of roll and yaw, then \p A will be of size 2x3. - * * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (roll, pitch, yaw) + * order (qx, qy, qz) * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) */ - NormalPriorOrientation3D(const fuse_core::MatrixXd & A, const fuse_core::Vector4d & b); + NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b); /** * @brief Destructor @@ -96,7 +90,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction -#include #include #include @@ -42,13 +40,10 @@ namespace fuse_constraints { -NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::MatrixXd & A, const fuse_core::Vector4d & b) +NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) : A_(A), b_(b) { - CHECK_GT(A_.rows(), 0); - CHECK_EQ(A_.cols(), 3); - set_num_residuals(A_.rows()); } bool NormalPriorOrientation3D::Evaluate( @@ -56,8 +51,6 @@ bool NormalPriorOrientation3D::Evaluate( double * residuals, double ** jacobians) const { - fuse_core::Vector3d full_residuals_vector; - double variable[4] = { parameters[0][0], @@ -79,16 +72,15 @@ bool NormalPriorOrientation3D::Evaluate( double j_quat2angle[12]; fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, &full_residuals_vector[0], j_quat2angle); // orientation angle-axis + fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map residuals_vector(residuals, num_residuals()); - residuals_vector = A_ * full_residuals_vector; + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); if (jacobians != nullptr) { - // Jacobian of the orientation residuals wrt orientation parameters block (max 3x4) - Eigen::Map j_map(jacobians[0], num_residuals(), 4); + Eigen::Map> j_map(jacobians[0]); Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); j_map = A_ * j_quat2angle_map * j_product_map; From 34ecebbaa4e1c4c1d9af3e7971188587a4a27c6c Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:13:11 +0000 Subject: [PATCH 069/116] normal prior orientation 3d cost functor cleanup --- .../normal_prior_orientation_3d_cost_functor.hpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp index 5b817731d..b2702ee2d 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -81,13 +80,11 @@ class NormalPriorOrientation3DCostFunctor * @param[in] b The orientation measurement or prior in order (w, x, y, z) */ NormalPriorOrientation3DCostFunctor( - const fuse_core::MatrixXd & A, + const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) : A_(A), b_(b) { - CHECK_GT(A_.rows(), 0); - CHECK_EQ(A_.cols(), 3); } /** @@ -99,8 +96,6 @@ class NormalPriorOrientation3DCostFunctor using fuse_variables::Orientation3DStamped; // Compute the delta quaternion - T full_residuals[3]; - T variable[4] = { orientation[0], @@ -119,19 +114,18 @@ class NormalPriorOrientation3DCostFunctor T difference[4]; ceres::QuaternionProduct(observation_inverse, variable, difference); - ceres::QuaternionToAngleAxis(difference, full_residuals); + ceres::QuaternionToAngleAxis(difference, residuals); // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. - Eigen::Map> full_residuals_map(full_residuals); - Eigen::Map> residuals_map(residuals, A_.rows()); - residuals_map = A_.template cast() * full_residuals_map; + Eigen::Map> residuals_map(residuals); + residuals_map.applyOnTheLeft(A_.template cast()); return true; } private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix fuse_core::Vector4d b_; //!< The measured 3D orientation (quaternion) value }; From 99c105508e275087f94fc1ae4b9f1578d2615e97 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:14:44 +0000 Subject: [PATCH 070/116] normal prior pose 3d cost functor cleanup --- .../fuse_constraints/normal_prior_pose_3d_cost_functor.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index 99e9b119f..f8d20f96f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics - * Copyrigth (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,7 +35,6 @@ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_HPP_ #include -#include #include #include @@ -120,8 +118,9 @@ bool NormalPriorPose3DCostFunctor::operator()( // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residual); - residuals_map.applyOnTheLeft(A_.template cast()); + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); + return true; } From ac54c93e47d57a25ab168f9877e11978f7e2ed98 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:15:03 +0000 Subject: [PATCH 071/116] normal prior pose 3d euler cost functor/function cleanup --- .../include/fuse_constraints/normal_prior_pose_3d_euler.hpp | 1 - .../normal_prior_pose_3d_euler_cost_functor.hpp | 2 -- fuse_constraints/src/normal_prior_pose_3d_euler.cpp | 3 +-- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 4d8fce419..0579b30c7 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp index e6eb471bb..085bc2fd6 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -36,7 +35,6 @@ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ #include -#include #include #include diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index e07415d37..c4871aa3b 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Clearpath Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -88,7 +87,7 @@ bool NormalPriorPose3DEuler::Evaluate( Eigen::Map> j_quat2angle_map(j_quat2rpy); Eigen::Map(jacobians[1], num_residuals(), 4) = A_.rightCols<3>() * j_quat2angle_map; - } + } } return true; } From 0e930cd6fed460dde7053e150e9f87cf281a42a3 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:17:28 +0000 Subject: [PATCH 072/116] relative pose 3d constraint cleanup --- .../relative_pose_3d_stamped_constraint.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index 52e4f503d..21155fc4c 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -93,7 +93,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation2, const fuse_core::Vector7d & delta, const fuse_core::Matrix6d & covariance); - + /** * @brief Destructor */ @@ -112,7 +112,10 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Compute the measurement covariance matrix. */ - fuse_core::Matrix6d covariance() const {return (sqrt_information_.transpose() * sqrt_information_).inverse();}; + fuse_core::Matrix6d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } /** * @brief Print a human-readable description of the constraint to the provided stream. From 7073ae7c1a4af8f996d034d877a9a58f968612fe Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:21:49 +0000 Subject: [PATCH 073/116] normal prior pose 3d cost function cleanup --- fuse_constraints/src/normal_prior_pose_3d.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index b8c724497..47eaf11bd 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Clearpath Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -33,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include #include #include @@ -74,9 +72,11 @@ bool NormalPriorPose3D::Evaluate( double j_product[16]; double j_quat2angle[12]; - residuals[0] = parameters[0][0] - b_[0]; // position x - residuals[1] = parameters[0][1] - b_[1]; // position y - residuals[2] = parameters[0][2] - b_[2]; // position z + // Compute position residuals + residuals[0] = parameters[0][0] - b_[0]; + residuals[1] = parameters[0][1] - b_[1]; + residuals[2] = parameters[0][2] - b_[2]; + // Compute orientation residuals fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis From 4b554dae565bde46ba616324dd0ae544829146dc Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:30:15 +0000 Subject: [PATCH 074/116] fuse_constraints tests cleanup --- .../test_absolute_orientation_3d_stamped_euler_constraint.cpp | 1 + .../test/test_absolute_pose_3d_stamped_euler_constraint.cpp | 1 - fuse_constraints/test/test_normal_prior_pose_3d.cpp | 1 - fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp | 1 - .../test/test_relative_pose_3d_stamped_euler_constraint.cpp | 1 - 5 files changed, 1 insertion(+), 4 deletions(-) diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index ab685ef3a..36f149456 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index b5ebb2110..2838692ac 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 202b0739d..a477c3f6a 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Clearpath Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index eb6d436f4..29e4766eb 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2020, Clearpath Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index 4fdacf208..fffe35642 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * From 666d6bc21833c15de41ec29bd60bb09c23ecf768 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:44:49 +0000 Subject: [PATCH 075/116] removed fuse_core::Quaternion alias --- fuse_core/include/fuse_core/eigen.hpp | 4 ---- fuse_models/include/fuse_models/unicycle_3d.hpp | 2 +- .../include/fuse_models/unicycle_3d_predict.hpp | 8 ++++---- fuse_models/src/odometry_3d_publisher.cpp | 2 +- fuse_models/src/unicycle_3d.cpp | 4 ++-- fuse_models/src/unicycle_3d_ignition.cpp | 2 +- fuse_models/test/test_sensor_proc_3d.cpp | 2 +- fuse_models/test/test_unicycle_3d.cpp | 14 +++++++------- fuse_models/test/test_unicycle_3d_predict.cpp | 6 +++--- 9 files changed, 20 insertions(+), 24 deletions(-) diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 93bf334df..447325539 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -55,7 +55,6 @@ using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; using Vector15d = Eigen::Matrix; -using Vector16d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; @@ -68,9 +67,6 @@ using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; using Matrix15d = Eigen::Matrix; -using Matrix16d = Eigen::Matrix; - -using Quaternion = Eigen::Quaternion; template using Matrix = Eigen::Matrix; diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/unicycle_3d.hpp index cd2acbac0..9134aa86b 100644 --- a/fuse_models/include/fuse_models/unicycle_3d.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d.hpp @@ -109,7 +109,7 @@ class Unicycle3D : public fuse_core::AsyncMotionModel fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration //!< variable fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position - fuse_core::Quaternion orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) + Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index afbf7cbef..cc08557b3 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -492,13 +492,13 @@ inline void predict( */ inline void predict( const fuse_core::Vector3d & position1, - const fuse_core::Quaternion & orientation1, + const Eigen::Quaterniond & orientation1, const fuse_core::Vector3d & vel_linear1, const fuse_core::Vector3d & vel_angular1, const fuse_core::Vector3d & acc_linear1, const double dt, fuse_core::Vector3d & position2, - fuse_core::Quaternion & orientation2, + Eigen::Quaterniond & orientation2, fuse_core::Vector3d & vel_linear2, fuse_core::Vector3d & vel_angular2, fuse_core::Vector3d & acc_linear2) @@ -565,13 +565,13 @@ inline void predict( */ inline void predict( const fuse_core::Vector3d & position1, - const fuse_core::Quaternion & orientation1, + const Eigen::Quaterniond & orientation1, const fuse_core::Vector3d & vel_linear1, const fuse_core::Vector3d & vel_angular1, const fuse_core::Vector3d & acc_linear1, const double dt, fuse_core::Vector3d & position2, - fuse_core::Quaternion & orientation2, + Eigen::Quaterniond & orientation2, fuse_core::Vector3d & vel_linear2, fuse_core::Vector3d & vel_angular2, fuse_core::Vector3d & acc_linear2, diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 12ee64062..0bb666d8c 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -476,7 +476,7 @@ void Odometry3DPublisher::publishTimerCallback() // Convert pose in Eigen representation fuse_core::Vector3d position, velocity_linear, velocity_angular; - fuse_core::Quaternion orientation; + Eigen::Quaterniond orientation; position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); orientation.x() = pose.getRotation().x(); orientation.y() = pose.getRotation().y(); diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index 8deb94af0..e784f8e70 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -70,7 +70,7 @@ inline bool isfinite(const fuse_core::Vector3d & vector) return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); } -inline bool isNormalized(const fuse_core::Quaternion & q) +inline bool isNormalized(const Eigen::Quaterniond & q) { return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < Eigen::NumTraits::dummy_precision(); @@ -83,7 +83,7 @@ std::string to_string(const fuse_core::Vector3d & vector) return oss.str(); } -std::string to_string(const fuse_core::Quaternion & quaternion) +std::string to_string(const Eigen::Quaterniond & quaternion) { std::ostringstream oss; oss << quaternion; diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp index 9c0167cc5..9518f2789 100644 --- a/fuse_models/src/unicycle_3d_ignition.cpp +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -408,7 +408,7 @@ void Unicycle3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( name(), *orientation, - fuse_core::Quaternion(orientation->w(), orientation->x(), orientation->y(), orientation->z()), + Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), orientation_cov); auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp index 29e39ff2c..c71d24bf6 100644 --- a/fuse_models/test/test_sensor_proc_3d.cpp +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -100,7 +100,7 @@ TEST(TestSuite, populatePartialMeasurements) // Test both conversion from quaternion to RPY and partial measurement population // This one is just to generate a random unit quaternion and have the reference in RPY fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); - fuse_core::Quaternion q = + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); diff --git a/fuse_models/test/test_unicycle_3d.cpp b/fuse_models/test/test_unicycle_3d.cpp index 7e1052f42..b8dfebfa9 100644 --- a/fuse_models/test/test_unicycle_3d.cpp +++ b/fuse_models/test/test_unicycle_3d.cpp @@ -165,7 +165,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) angular_velocity1->uuid(), linear_acceleration1->uuid(), fuse_core::Vector3d(1.0, 0.0, 0.0), - fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) @@ -178,7 +178,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) angular_velocity2->uuid(), linear_acceleration2->uuid(), fuse_core::Vector3d(2.0, 0.0, 0.0), - fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) @@ -191,7 +191,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) angular_velocity3->uuid(), linear_acceleration3->uuid(), fuse_core::Vector3d(3.0, 0.0, 0.0), - fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) @@ -204,7 +204,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) angular_velocity4->uuid(), linear_acceleration4->uuid(), fuse_core::Vector3d(4.0, 0.0, 0.0), - fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) @@ -217,7 +217,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) angular_velocity5->uuid(), linear_acceleration5->uuid(), fuse_core::Vector3d(5.0, 0.0, 0.0), - fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) @@ -231,7 +231,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) { // The first entry is missing from the graph. It will not get updated. auto expected_position = fuse_core::Vector3d(1.0, 0.0, 0.0); - auto expected_orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); auto actual_position = state_history[rclcpp::Time(1, 0)].position; auto actual_orientation = state_history[rclcpp::Time(1, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -263,7 +263,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) { // The second entry is included in the graph. It will get updated directly. auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph - auto expected_orientation = fuse_core::Quaternion(1.0, 0.0, 0.0, 0.0); + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); auto actual_position = state_history[rclcpp::Time(2, 0)].position; auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index 314f4fbc2..0b47a97c9 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -44,13 +44,13 @@ TEST(Predict, predictDirectVals) { fuse_core::Vector3d position1 (0.0, 0.0, 0.0); - fuse_core::Quaternion orientation1 (1.0, 0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1 (1.0, 0.0, 0.0, 0.0); fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); double dt = 0.1; fuse_core::Vector3d position2; - fuse_core::Quaternion orientation2; + Eigen::Quaterniond orientation2; fuse_core::Vector3d vel_linear2; fuse_core::Vector3d vel_angular2; fuse_core::Vector3d acc_linear2; @@ -68,7 +68,7 @@ TEST(Predict, predictDirectVals) vel_angular2, acc_linear2); - fuse_core::Quaternion q; + Eigen::Quaterniond q; q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); From 73d9178fe2e8c74171cee2ae740163164688aa9b Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 17:51:09 +0000 Subject: [PATCH 076/116] sensor_proc cleanup --- fuse_models/include/fuse_models/common/sensor_proc.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 1d614c224..c229a3519 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -1012,6 +1013,7 @@ inline bool processDifferentialPose3DWithCovariance( const bool validate, fuse_core::Transaction & transaction) { + // Here we are using covariance_geometry types to compute the relative pose covariance: // PoseQuaternionCovarianceRPY is std::pair, Covariance> // Position is Eigen::Vector3d // Quaternion is Eigen::Quaterniond From 23d5773bf6bb1838ab33d4507133fd24bdfde8e8 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:09:58 +0000 Subject: [PATCH 077/116] unicycle2D cleanup --- fuse_models/src/unicycle_2d.cpp | 2 +- .../src/unicycle_2d_state_kinematic_constraint.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index c6dba7484..00f71e18a 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -340,7 +340,7 @@ void Unicycle2D::generateMotionModel( state2.velocity_linear, state2.velocity_yaw, state2.acceleration_linear); - + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 3ad47a4ad..be454a336 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -38,8 +38,7 @@ #include #include -#include -// #include +#include #include #include #include @@ -105,13 +104,13 @@ ceres::CostFunction * Unicycle2DStateKinematicConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // - return new ceres::AutoDiffCostFunction( new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); + // return new ceres::AutoDiffCostFunction( new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); // // which requires: // // #include - // return new Unicycle2DStateCostFunction(dt_, sqrt_information_); + return new Unicycle2DStateCostFunction(dt_, sqrt_information_); } } // namespace fuse_models From 1353917e3ef0e56b3081652479eac49568c9755a Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:10:59 +0000 Subject: [PATCH 078/116] unicycle2d state kinematic constraint cleanup --- fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index be454a336..9accc5061 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include #include #include From e66dadba488e4291d4a6faa6385c36892ecaa4e4 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:17:11 +0000 Subject: [PATCH 079/116] 3d models cleanup --- fuse_models/include/fuse_models/imu_3d.hpp | 2 +- .../include/fuse_models/odometry_3d.hpp | 2 +- .../fuse_models/odometry_3d_publisher.hpp | 2 +- .../fuse_models/parameters/imu_3d_params.hpp | 2 +- .../parameters/odometry_3d_params.hpp | 2 +- .../odometry_3d_publisher_params.hpp | 2 +- .../unicycle_3d_ignition_params.hpp | 2 +- .../include/fuse_models/unicycle_3d.hpp | 2 +- .../fuse_models/unicycle_3d_ignition.hpp | 2 +- .../fuse_models/unicycle_3d_predict.hpp | 2 - .../unicycle_3d_state_cost_function.hpp | 3 +- .../unicycle_3d_state_cost_functor.hpp | 38 +++++++++++-------- ...unicycle_3d_state_kinematic_constraint.hpp | 30 +++++++-------- fuse_models/src/imu_3d.cpp | 2 +- fuse_models/src/odometry_3d.cpp | 2 +- fuse_models/src/odometry_3d_publisher.cpp | 2 +- fuse_models/src/unicycle_3d.cpp | 2 +- fuse_models/src/unicycle_3d_ignition.cpp | 2 +- ...unicycle_3d_state_kinematic_constraint.cpp | 8 ++-- 19 files changed, 57 insertions(+), 52 deletions(-) diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 5095916a6..8dc570000 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 6f932595d..8a888f3a8 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index 913e9a511..f69caae0f 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 9e08c7991..1c0dda5c5 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 647846484..1e01a073c 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index f4acf4de8..64d2d1380 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp index 4405616f5..8bf946f8d 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/unicycle_3d.hpp index 9134aa86b..0addd4070 100644 --- a/fuse_models/include/fuse_models/unicycle_3d.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp index 16080852a..5dba3b875 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp index cc08557b3..1a9a3501d 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.hpp @@ -1,7 +1,6 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * @@ -269,7 +268,6 @@ inline void predict( fuse_core::wrapAngle2D(orientation2_p); fuse_core::wrapAngle2D(orientation2_y); - // TODO(giafranchini): should we store common expressions? if (jacobians) { // Jacobian wrt position1 if (jacobians[0]) { diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp index 051873d1c..29048e7ff 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -140,7 +140,6 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, double vel_linear_pred[3]; double vel_angular_pred[3]; double acc_linear_pred[3]; - double orientation1_rpy[3]; double orientation2_rpy[3]; double j1_quat2rpy[12]; diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp index f96676936..b632ecb44 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -48,25 +48,30 @@ namespace fuse_models * @brief Create a cost function for a 3D state vector * * The state vector includes the following quantities, given in this order: - * pose translation x, y, z - * pose orientation (in quaternion) w, x, y, z - * linear velocity x, y, z - * angular velocity x, y, z - * linear acceleration x, y, z + * position (x, y, z) + * orientation (roll, pitch, yaw) + * linear velocity (x, y, z) + * angular velocity (roll, pitch, yaw) + * linear acceleration (x, y, z) * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost * function that applies a prior constraint on both the entire state vector. * * The cost function is of the form: * - * || [ x_t2 - proj(x_t1) ] ||^2 - * cost(x) = || [ y_t2 - proj(y_t1) ] || - * || [ yaw_t2 - proj(yaw_t1) ] || - * ||A * [ x_vel_t2 - proj(x_vel_t1) ] || - * || [ y_vel_t2 - proj(y_vel_t1) ] || - * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || - * || [ x_acc_t2 - proj(x_acc_t1) ] || - * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ x_t2 - proj(x_t1) ] ||^2 + * || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ quat2eul(rpy_t2) - proj(quat2eul(rpy_t1)) ] || + * cost(x) = || A *[ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ z_vel_t2 - proj(z_vel_t1) ] || + * || [ roll_vel_t2 - proj(roll_vel_t1) ] || + * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ z_acc_t2 - proj(z_acc_t1) ] || * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and * proj is a function that projects the state variables from time t1 to time t2. In case the user is @@ -87,7 +92,8 @@ class Unicycle3DStateCostFunctor * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) */ Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); @@ -102,7 +108,7 @@ class Unicycle3DStateCostFunctor * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) - * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ template diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp index f2c306e4d..e7d313a36 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -79,20 +79,20 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint * The constraint is created between two states. The state is broken up into multiple fuse * variable types. * - * @param[in] source The name of the sensor or motion model that generated this constraint - * @param[in] position1 Position component variable of the fist state - * @param[in] orientation1 Orientation component variable of the first state - * @param[in] velocity_linear1 Linear velocity component variable of the first state - * @param[in] velocity_angular1 Angular velocity component variable of the first state - * @param[in] acceleration_linear1 Linear acceleration component variable of the first state - * @param[in] position2 Position component variable of the second state - * @param[in] orientation2 Orientation component variable of the second state - * @param[in] velocity_linear2 Linear velocity component variable of the second state - * @param[in] velocity_angular2 Angular velocity component variable of the second state - * @param[in] acceleration_linear2 Linear acceleration component variable of the second state - * @param[in] covariance - The covariance matrix used to weight the constraint. Order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, - * x_acc, y_acc, z_acc) + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 Position component variable of the fist state + * @param[in] orientation1 Orientation component variable of the first state + * @param[in] velocity_linear1 Linear velocity component variable of the first state + * @param[in] velocity_angular1 Angular velocity component variable of the first state + * @param[in] acceleration_linear1 Linear acceleration component variable of the first state + * @param[in] position2 Position component variable of the second state + * @param[in] orientation2 Orientation component variable of the second state + * @param[in] velocity_linear2 Linear velocity component variable of the second state + * @param[in] velocity_angular2 Angular velocity component variable of the second state + * @param[in] acceleration_linear2 Linear acceleration component variable of the second state + * @param[in] covariance The covariance matrix used to weight the constraint. Order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) */ Unicycle3DStateKinematicConstraint( const std::string & source, diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 4f60f0890..9fdc11d4f 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index ca604a7ed..85cb1cc7f 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 0bb666d8c..48b6894dc 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp index e784f8e70..6c6588811 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/unicycle_3d.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp index 9518f2789..648c15b5c 100644 --- a/fuse_models/src/unicycle_3d_ignition.cpp +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp index 26e6084f1..0fe1f3a46 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,13 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -// #include #include #include #include #include -// #include #include #include #include @@ -107,6 +105,10 @@ ceres::CostFunction * Unicycle3DStateKinematicConstraint::costFunction() const // // return new ceres::AutoDiffCostFunction( // new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); + + // And including the followings: + // #include + // #include } } // namespace fuse_models From b104d77417eca188472878663d2520b585d13723 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Thu, 11 Jan 2024 19:26:24 +0100 Subject: [PATCH 080/116] Update optimizer.hpp --- fuse_optimizers/include/fuse_optimizers/optimizer.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index 0cc1c956a..ce6dc6729 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -98,7 +98,7 @@ class Optimizer public: FUSE_SMART_PTR_ALIASES_ONLY(Optimizer) - /** + /** * @brief Constructor * * @param[in] interfaces The node interfaces for the node driving the optimizer @@ -115,7 +115,6 @@ class Optimizer virtual ~Optimizer(); protected: - // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type // rather annoying as it is not equivalent to Class::UniquePtr using MotionModelUniquePtr = class_loader::ClassLoader::UniquePtr; From 7a2a338202eb9c3644c57dca3aea209cf61615c8 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Thu, 11 Jan 2024 19:28:43 +0100 Subject: [PATCH 081/116] Update test_sensor_proc.cpp --- fuse_models/test/test_sensor_proc.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 921d67c54..637a93699 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -11,6 +11,7 @@ #include namespace fm_common = fuse_models::common; + TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) { const std::vector position_indices{0, 1}; From 43667496f11719a13bacb04b3cf7848647ff007e Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:34:04 +0000 Subject: [PATCH 082/116] removed utils.py --- fuse_models/test/utils.py | 93 --------------------------------------- 1 file changed, 93 deletions(-) delete mode 100644 fuse_models/test/utils.py diff --git a/fuse_models/test/utils.py b/fuse_models/test/utils.py deleted file mode 100644 index bca5644bb..000000000 --- a/fuse_models/test/utils.py +++ /dev/null @@ -1,93 +0,0 @@ -import numpy as np -import quaternion -from math import sin, cos, tan - -p1x = 0.0 -p1y = 0.0 -p1z = 0.0 -r1 = -2.490 -p1 = -0.206 -y1 = 3.066 -v1x = 0.1 -v1y = 0.2 -v1z = 0.1 -w1x = 1.570796327 -w1y = 1.570796327 -w1z = -1.570796327 -a1x = -0.5 -a1y = 1.0 -a1z = 1.0 -dt = 0.1 - -def get_quaternion(roll, pitch, yaw): - w = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/ 2) * sin(pitch / 2) * sin(yaw / 2) - x = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2) - y = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - z = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2) - q = np.quaternion() - q.w = w - q.x = x - q.y = y - q.z = z - q = q.normalized() - return q - - -def main(): - p2x = p1x + \ - ((cos(y1)*cos(p1))*v1x + (cos(y1)*sin(p1)*sin(r1) - sin(y1)*cos(r1))*v1y + (cos(y1)*sin(p1)*cos(r1) + sin(y1)*sin(r1))*v1z) * dt + \ - ((cos(y1)*cos(p1))*a1x + (cos(y1)*sin(p1)*sin(r1) - sin(y1)*cos(r1))*a1y + (cos(y1)*sin(p1)*cos(r1) + sin(y1)*sin(r1))*a1z) * dt * dt * 0.5 - - p2y = p1y + \ - ((sin(y1)*cos(p1))*v1x + (sin(y1)*sin(p1)*sin(r1) + cos(y1)*cos(r1))*v1y + (sin(y1)*sin(p1)*cos(r1) - cos(y1)*sin(r1))*v1z) * dt + \ - ((sin(y1)*cos(p1))*a1x + (sin(y1)*sin(p1)*sin(r1) + cos(y1)*cos(r1))*a1y + (sin(y1)*sin(p1)*cos(r1) - cos(y1)*sin(r1))*a1z) * dt * dt * 0.5 - - p2z = p1z + \ - ((-sin(p1))*v1x + (cos(p1)*sin(r1))*v1y + (cos(p1)*cos(r1))*v1z) * dt + \ - ((-sin(p1))*a1x + (cos(p1)*sin(r1))*a1y + (cos(p1)*cos(r1))*a1z) * dt * dt * 0.5 - - r2 = r1 + \ - (w1x + (sin(r1)*tan(p1))*w1y + (cos(r1)*tan(p1))*w1z) * dt - - p2 = p1 + \ - ((cos(r1))*w1y + (-sin(r1))*w1z) * dt - - y2 = y1 + \ - ((sin(r1)/cos(p1))*w1y + (cos(r1)/cos(p1))*w1z) * dt - - q2 = get_quaternion(r2, p2, y2) - - v2x = v1x + a1x * dt - v2y = v1y + a1y * dt - v2z = v1z + a1z * dt - - w2x = w1x - w2y = w1y - w2z = w1z - - a2x = a1x - a2y = a1y - a2z = a1z - - print("p2x: ", p2x) - print("p2y: ", p2y) - print("p2z: ", p2z) - print("r2: ", r2) - print("p2: ", p2) - print("y2: ", y2) - print("q2w: ", q2.w) - print("q2x: ", q2.x) - print("q2y: ", q2.y) - print("q2z: ", q2.z) - print("v2x: ", v2x) - print("v2y: ", v2y) - print("v2z: ", v2z) - print("w2x: ", w2x) - print("w2y: ", w2y) - print("w2z: ", w2z) - print("a2x: ", a2x) - print("a2y: ", a2y) - print("a2z: ", a2z) - -if __name__ == '__main__': - main() \ No newline at end of file From 34a3ab36c0dc651368898291282260d4f4357194 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:41:54 +0000 Subject: [PATCH 083/116] fuse optimizers cleanup --- fuse_optimizers/CMakeLists.txt | 8 -------- fuse_optimizers/src/fixed_lag_smoother.cpp | 8 +------- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 2b26f549c..75fa7219d 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -54,14 +54,6 @@ target_link_libraries(${PROJECT_NAME} ) ament_target_dependencies(${PROJECT_NAME} diagnostic_updater) -## batch_optimizer node -# add_executable(batch_optimizer_node src/batch_optimizer_node.cpp) -# target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) - -## fixed_lag_smoother node -# add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) -# target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) - ## batch_optimizer component add_library(batch_optimizer_component SHARED src/batch_optimizer_component.cpp) target_include_directories(batch_optimizer_component PRIVATE include) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 424be020a..f80960862 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -196,7 +196,6 @@ void FixedLagSmoother::optimizationLoop() while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) { - RCLCPP_DEBUG(logger_, "In the optimization loop"); // Wait for the next signal to start the next optimization cycle // NOTE(CH3): Uninitialized, but it's ok since it's meant to get overwritten. auto optimization_deadline = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -214,7 +213,6 @@ void FixedLagSmoother::optimizationLoop() } // Optimize { - RCLCPP_DEBUG(logger_, "optimizing"); std::lock_guard lock(optimization_mutex_); // Apply motion models auto new_transaction = fuse_core::Transaction::make_shared(); @@ -255,8 +253,6 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); - // std::cout << summary_.FullReport() << std::endl; - // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); @@ -294,9 +290,7 @@ void FixedLagSmoother::optimizationLoop() << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); } } - RCLCPP_DEBUG(logger_, "Ending while loop"); } - RCLCPP_DEBUG(logger_, "optimization finished"); } void FixedLagSmoother::optimizerTimerCallback() @@ -406,6 +400,7 @@ void FixedLagSmoother::processQueue( return; } } + // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); @@ -505,7 +500,6 @@ void FixedLagSmoother::transactionCallback( fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it - RCLCPP_DEBUG(logger_, "transactionCallback: inside transactionCallback"); auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { From 235bd94bc1761b4dcb5f41ee35ba0970bb5a1880 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:43:45 +0000 Subject: [PATCH 084/116] range_sensor_tutorial launch file cleanup --- fuse_tutorials/launch/range_sensor_tutorial.launch.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/fuse_tutorials/launch/range_sensor_tutorial.launch.py b/fuse_tutorials/launch/range_sensor_tutorial.launch.py index 6a94db83e..12da87712 100755 --- a/fuse_tutorials/launch/range_sensor_tutorial.launch.py +++ b/fuse_tutorials/launch/range_sensor_tutorial.launch.py @@ -42,8 +42,6 @@ def generate_launch_description(): parameters=[PathJoinSubstitution([ pkg_dir, 'config', 'range_sensor_tutorial.yaml' ])], - # prefix=["gdbserver localhost:3000"], - emulate_tty=True, ), Node( package='rviz2', From 238b7d4bd85d7d68d315eaa3078ae7ee77cd3908 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Thu, 11 Jan 2024 19:44:21 +0100 Subject: [PATCH 085/116] Update range_sensor_tutorial.launch.py --- fuse_tutorials/launch/range_sensor_tutorial.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_tutorials/launch/range_sensor_tutorial.launch.py b/fuse_tutorials/launch/range_sensor_tutorial.launch.py index 12da87712..ce708483e 100755 --- a/fuse_tutorials/launch/range_sensor_tutorial.launch.py +++ b/fuse_tutorials/launch/range_sensor_tutorial.launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): output='screen', parameters=[PathJoinSubstitution([ pkg_dir, 'config', 'range_sensor_tutorial.yaml' - ])], + ])] ), Node( package='rviz2', From 0515c902e6b2f508629102a15d3a39ecd40ae212 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 18:45:36 +0000 Subject: [PATCH 086/116] orientation_3d variable cleanup --- .../include/fuse_variables/orientation_3d_stamped.hpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 54581e090..65f5810cc 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -58,7 +58,7 @@ namespace fuse_variables /** * @brief A LocalParameterization class for 3D Orientations. * - * 3D orientations add and subtract nonlinearly. Additionally, the typical 3D orientation + * 3D orientations add and subtract nonlinearly. Additionally, the typcial 3D orientation * representation is a quaternion, which has 4 degrees of freedom to parameterize a 3D space. This * local parameterization uses the Rodrigues/angle-axis formulas to combine 3D rotations, along with * the appropriate "analytic" derivatives. @@ -82,28 +82,21 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati int GlobalSize() const override { - // the manifold is the quaternion space, which is 4D return 4; } int LocalSize() const override { - // the local tangent space is the angle-axis space, which is 3D return 3; } - // the box-plus operator: given a quaternion x and a tangent vector delta, this function computes - // the perturbed quaternion x_plus_delta bool Plus( const double * x, const double * delta, double * x_plus_delta) const override { double q_delta[4]; - // exponential map: transform a delta in tangent space into a delta in the manifold ceres::AngleAxisToQuaternion(delta, q_delta); - // manifold group product operation (in this case is the quaternion product) - // this combines the delta with the current value ceres::QuaternionProduct(x, q_delta, x_plus_delta); return true; } From 72ff6a9f5a8f518062b0f2505e3790641d62bccd Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 11 Jan 2024 19:14:47 +0000 Subject: [PATCH 087/116] rename unicycle3d to omnidirectional3d --- fuse_models/CMakeLists.txt | 6 +-- fuse_models/fuse_plugins.xml | 8 ++-- .../fuse_models/odometry_3d_publisher.hpp | 2 +- ...unicycle_3d.hpp => omnidirectional_3d.hpp} | 10 ++--- ...on.hpp => omnidirectional_3d_ignition.hpp} | 22 +++++----- ...ict.hpp => omnidirectional_3d_predict.hpp} | 0 ...mnidirectional_3d_state_cost_function.hpp} | 8 ++-- ...omnidirectional_3d_state_cost_functor.hpp} | 10 ++--- ...ctional_3d_state_kinematic_constraint.hpp} | 12 +++--- ...=> omnidirectional_3d_ignition_params.hpp} | 4 +- fuse_models/src/odometry_3d_publisher.cpp | 2 +- ...unicycle_3d.cpp => omnidirectional_3d.cpp} | 40 +++++++++---------- ...on.cpp => omnidirectional_3d_ignition.cpp} | 30 +++++++------- ...ctional_3d_state_kinematic_constraint.cpp} | 22 +++++----- fuse_models/test/CMakeLists.txt | 6 +-- ...cle_3d.cpp => test_omnidirectional_3d.cpp} | 26 ++++++------ ...pp => test_omnidirectional_3d_predict.cpp} | 2 +- ...mnidirectional_3d_state_cost_function.cpp} | 10 ++--- 18 files changed, 110 insertions(+), 110 deletions(-) rename fuse_models/include/fuse_models/{unicycle_3d.hpp => omnidirectional_3d.hpp} (97%) rename fuse_models/include/fuse_models/{unicycle_3d_ignition.hpp => omnidirectional_3d_ignition.hpp} (92%) rename fuse_models/include/fuse_models/{unicycle_3d_predict.hpp => omnidirectional_3d_predict.hpp} (100%) rename fuse_models/include/fuse_models/{unicycle_3d_state_cost_function.hpp => omnidirectional_3d_state_cost_function.hpp} (97%) rename fuse_models/include/fuse_models/{unicycle_3d_state_cost_functor.hpp => omnidirectional_3d_state_cost_functor.hpp} (96%) rename fuse_models/include/fuse_models/{unicycle_3d_state_kinematic_constraint.hpp => omnidirectional_3d_state_kinematic_constraint.hpp} (94%) rename fuse_models/include/fuse_models/parameters/{unicycle_3d_ignition_params.hpp => omnidirectional_3d_ignition_params.hpp} (98%) rename fuse_models/src/{unicycle_3d.cpp => omnidirectional_3d.cpp} (95%) rename fuse_models/src/{unicycle_3d_ignition.cpp => omnidirectional_3d_ignition.cpp} (95%) rename fuse_models/src/{unicycle_3d_state_kinematic_constraint.cpp => omnidirectional_3d_state_kinematic_constraint.cpp} (83%) rename fuse_models/test/{test_unicycle_3d.cpp => test_omnidirectional_3d.cpp} (95%) rename fuse_models/test/{test_unicycle_3d_predict.cpp => test_omnidirectional_3d_predict.cpp} (99%) rename fuse_models/test/{test_unicycle_3d_state_cost_function.cpp => test_omnidirectional_3d_state_cost_function.cpp} (93%) diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index dba1babd4..0df2d164d 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -55,9 +55,9 @@ add_library(${PROJECT_NAME} src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp - src/unicycle_3d.cpp - src/unicycle_3d_ignition.cpp - src/unicycle_3d_state_kinematic_constraint.cpp + src/omnidirectional_3d.cpp + src/omnidirectional_3d_ignition.cpp + src/omnidirectional_3d_state_kinematic_constraint.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 4a8003d34..7830e9a81 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -12,13 +12,13 @@ - + A class that represents a kinematic constraint between 3D states at two different times. - + A fuse_models 3D kinematic model that generates kinematic constraints between provided time stamps, and adds those constraints to the fuse graph. @@ -101,9 +101,9 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. - + - A fuse_models ignition sensor designed to be used in conjunction with the unicycle 3D motion model. + A fuse_models ignition sensor designed to be used in conjunction with the Omnidirectional 3D motion model. diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index f69caae0f..f1c70f441 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -74,7 +74,7 @@ namespace fuse_models * the tf tree * - predict_to_current_time (bool, default: false) The tf publication happens at a fixed rate. * This parameter specifies whether we should - * predict, using the 3D unicycle model, the state + * predict, using the 3D omnidirectional model, the state * at the time of the tf publication, rather than * the last posterior (optimized) state. * - publish_frequency (double, default: 10.0) How often, in Hz, we publish the filtered state diff --git a/fuse_models/include/fuse_models/unicycle_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp similarity index 97% rename from fuse_models/include/fuse_models/unicycle_3d.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d.hpp index 0addd4070..f8e4ccab3 100644 --- a/fuse_models/include/fuse_models/unicycle_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -70,22 +70,22 @@ namespace fuse_models * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc). */ -class Unicycle3D : public fuse_core::AsyncMotionModel +class Omnidirectional3D : public fuse_core::AsyncMotionModel { public: - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Unicycle3D) + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Omnidirectional3D) /** * @brief Default constructor * * All plugins are required to have a constructor that accepts no arguments */ - Unicycle3D(); + Omnidirectional3D(); /** * @brief Destructor */ - ~Unicycle3D() = default; + ~Omnidirectional3D() = default; /** * @brief Shadowing extension to the AsyncMotionModel::initialize call @@ -237,7 +237,7 @@ class Unicycle3D : public fuse_core::AsyncMotionModel StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Unicycle3D & unicycle_2d); +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & unicycle_2d); } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp similarity index 92% rename from fuse_models/include/fuse_models/unicycle_3d_ignition.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index 5dba3b875..ecbf39028 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include @@ -54,11 +54,11 @@ namespace fuse_models { /** - * @brief A fuse_models ignition sensor designed to be used in conjunction with the unicycle 3D + * @brief A fuse_models ignition sensor designed to be used in conjunction with the Omnidirectional 3D * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * unicycle 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this @@ -90,23 +90,23 @@ namespace fuse_models * - ~topic (string, default: "set_pose") The topic name for received PoseWithCovarianceStamped * messages */ -class Unicycle3DIgnition : public fuse_core::AsyncSensorModel +class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel { public: - FUSE_SMART_PTR_DEFINITIONS(Unicycle3DIgnition) - using ParameterType = parameters::Unicycle3DIgnitionParams; + FUSE_SMART_PTR_DEFINITIONS(Omnidirectional3DIgnition) + using ParameterType = parameters::Omnidirectional3DIgnitionParams; /** * @brief Default constructor * * All plugins are required to have a constructor that accepts no arguments */ - Unicycle3DIgnition(); + Omnidirectional3DIgnition(); /** * @brief Destructor */ - ~Unicycle3DIgnition() = default; + ~Omnidirectional3DIgnition() = default; /** * @brief Shadowing extension to the AsyncSensorModel::initialize call @@ -120,7 +120,7 @@ class Unicycle3DIgnition : public fuse_core::AsyncSensorModel * @brief Subscribe to the input topic to start sending transactions to the optimizer * * As a very special case, we are overriding the start() method instead of providing an onStart() - * implementation. This is because the Unicycle3DIgnition sensor calls reset() on the optimizer, + * implementation. This is because the Omnidirectional3DIgnition sensor calls reset() on the optimizer, * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of * start() and stop(), the system would hang inside of one callback function while waiting for * another callback to complete. @@ -131,7 +131,7 @@ class Unicycle3DIgnition : public fuse_core::AsyncSensorModel * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer * * As a very special case, we are overriding the stop() method instead of providing an onStop() - * implementation. This is because the Unicycle3DIgnition sensor calls reset() on the optimizer, + * implementation. This is because the Omnidirectional3DIgnition sensor calls reset() on the optimizer, * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of * start() and stop(), the system would hang inside of one callback function while waiting for * another callback to complete. @@ -180,7 +180,7 @@ class Unicycle3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Create and send a prior transaction based on the supplied pose * - * The unicycle 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the * parameter server. * diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp similarity index 100% rename from fuse_models/include/fuse_models/unicycle_3d_predict.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp similarity index 97% rename from fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 29048e7ff..730d48def 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -36,7 +36,7 @@ #include -#include +#include #include #include @@ -84,7 +84,7 @@ namespace fuse_models * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the * square root information matrix (the inverse of the covariance). */ -class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> +class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> { public: FUSE_MAKE_ALIGNED_OPERATOR_NEW() @@ -97,7 +97,7 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Unicycle3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -279,7 +279,7 @@ class Unicycle3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, //!< information matrix }; -Unicycle3DStateCostFunction::Unicycle3DStateCostFunction( +Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction( const double dt, const fuse_core::Matrix15d & A) : dt_(dt), diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp similarity index 96% rename from fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index b632ecb44..d95523a01 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -34,7 +34,7 @@ #ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ #define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ -#include +#include #include #include @@ -82,7 +82,7 @@ namespace fuse_models * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the * square root information matrix (the inverse of the covariance). */ -class Unicycle3DStateCostFunctor +class Omnidirectional3DStateCostFunctor { public: FUSE_MAKE_ALIGNED_OPERATOR_NEW() @@ -95,7 +95,7 @@ class Unicycle3DStateCostFunctor * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -131,7 +131,7 @@ class Unicycle3DStateCostFunctor //!< information matrix }; -Unicycle3DStateCostFunctor::Unicycle3DStateCostFunctor( +Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor( const double dt, const fuse_core::Matrix15d & A) : dt_(dt), @@ -140,7 +140,7 @@ Unicycle3DStateCostFunctor::Unicycle3DStateCostFunctor( } template -bool Unicycle3DStateCostFunctor::operator()( +bool Omnidirectional3DStateCostFunctor::operator()( const T * const position1, const T * const orientation1, const T * const vel_linear1, diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp similarity index 94% rename from fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp rename to fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index e7d313a36..a61de7bab 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -63,15 +63,15 @@ namespace fuse_models * The fuse_models 3D state is a combination of 3D position, 3D orientation, 3D linear velocity, 3D * angular velocity, and 3D linear acceleration. */ -class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint +class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint { public: - FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Unicycle3DStateKinematicConstraint) + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Omnidirectional3DStateKinematicConstraint) /** * @brief Default constructor */ - Unicycle3DStateKinematicConstraint() = default; + Omnidirectional3DStateKinematicConstraint() = default; /** * @brief Create a constraint using a time delta and a kinematic model cost functor @@ -94,7 +94,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Unicycle3DStateKinematicConstraint( + Omnidirectional3DStateKinematicConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, @@ -111,7 +111,7 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint /** * @brief Destructor */ - virtual ~Unicycle3DStateKinematicConstraint() = default; + virtual ~Omnidirectional3DStateKinematicConstraint() = default; /** * @brief Read-only access to the time delta between the first and second state (really, between @@ -183,6 +183,6 @@ class Unicycle3DStateKinematicConstraint : public fuse_core::Constraint } // namespace fuse_models -BOOST_CLASS_EXPORT_KEY(fuse_models::Unicycle3DStateKinematicConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_models::Omnidirectional3DStateKinematicConstraint); #endif // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp similarity index 98% rename from fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp rename to fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index 8bf946f8d..053ac5572 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -52,9 +52,9 @@ namespace parameters { /** - * @brief Defines the set of parameters required by the Unicycle3DIgnition class + * @brief Defines the set of parameters required by the Omnidirectional3DIgnition class */ -struct Unicycle3DIgnitionParams : public ParameterBase +struct Omnidirectional3DIgnitionParams : public ParameterBase { public: /** diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 48b6894dc..6fb94e5ee 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp similarity index 95% rename from fuse_models/src/unicycle_3d.cpp rename to fuse_models/src/omnidirectional_3d.cpp index 6c6588811..92534d25d 100644 --- a/fuse_models/src/unicycle_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -47,9 +47,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -60,7 +60,7 @@ #include // Register this motion model with ROS as a plugin. -PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3D, fuse_core::MotionModel) +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3D, fuse_core::MotionModel) namespace std { @@ -117,16 +117,16 @@ inline void validateCovariance( namespace fuse_models { -Unicycle3D::Unicycle3D() +Omnidirectional3D::Omnidirectional3D() : fuse_core::AsyncMotionModel(1), // Thread count = 1 for local callback queue logger_(rclcpp::get_logger("uninitialized")), buffer_length_(rclcpp::Duration::max()), device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Unicycle3D::generateMotionModel, this, rclcpp::Duration::max()) + timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) { } -void Unicycle3D::print(std::ostream & stream) const +void Omnidirectional3D::print(std::ostream & stream) const { stream << "state history:\n"; for (const auto & state : state_history_) { @@ -135,7 +135,7 @@ void Unicycle3D::print(std::ostream & stream) const } } -void Unicycle3D::StateHistoryElement::print(std::ostream & stream) const +void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const { stream << " position uuid: " << position_uuid << "\n" << " orientation uuid: " << orientation_uuid << "\n" @@ -149,7 +149,7 @@ void Unicycle3D::StateHistoryElement::print(std::ostream & stream) const << " acceleration linear: " << acc_linear << "\n"; } -void Unicycle3D::StateHistoryElement::validate() const +void Omnidirectional3D::StateHistoryElement::validate() const { if (!std::isfinite(position)) { throw std::runtime_error("Invalid position " + std::to_string(position)); @@ -168,7 +168,7 @@ void Unicycle3D::StateHistoryElement::validate() const } } -bool Unicycle3D::applyCallback(fuse_core::Transaction & transaction) +bool Omnidirectional3D::applyCallback(fuse_core::Transaction & transaction) { // Use the timestamp manager to generate just the required motion model segments. The timestamp // manager, in turn, makes calls to the generateMotionModel() function. @@ -184,12 +184,12 @@ bool Unicycle3D::applyCallback(fuse_core::Transaction & transaction) return true; } -void Unicycle3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) +void Omnidirectional3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) { updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } -void Unicycle3D::initialize( +void Omnidirectional3D::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string & name) { @@ -197,7 +197,7 @@ void Unicycle3D::initialize( fuse_core::AsyncMotionModel::initialize(interfaces, name); } -void Unicycle3D::onInit() +void Omnidirectional3D::onInit() { logger_ = interfaces_.get_node_logging_interface()->get_logger(); clock_ = interfaces_.get_node_clock_interface()->get_clock(); @@ -264,13 +264,13 @@ void Unicycle3D::onInit() device_id_ = fuse_variables::loadDeviceId(interfaces_); } -void Unicycle3D::onStart() +void Omnidirectional3D::onStart() { timestamp_manager_.clear(); state_history_.clear(); } -void Unicycle3D::generateMotionModel( +void Omnidirectional3D::generateMotionModel( const rclcpp::Time & beginning_stamp, const rclcpp::Time & ending_stamp, std::vector & constraints, @@ -458,7 +458,7 @@ void Unicycle3D::generateMotionModel( } // Create the constraints for this motion model segment - auto constraint = fuse_models::Unicycle3DStateKinematicConstraint::make_shared( + auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( name(), *position1, *orientation1, @@ -486,7 +486,7 @@ void Unicycle3D::generateMotionModel( variables.push_back(acceleration_linear2); } -void Unicycle3D::updateStateHistoryEstimates( +void Omnidirectional3D::updateStateHistoryEstimates( const fuse_core::Graph & graph, StateHistory & state_history, const rclcpp::Duration & buffer_length) @@ -592,7 +592,7 @@ void Unicycle3D::updateStateHistoryEstimates( } } -void Unicycle3D::validateMotionModel( +void Omnidirectional3D::validateMotionModel( const StateHistoryElement & state1, const StateHistoryElement & state2, const fuse_core::Matrix15d & process_noise_covariance) { @@ -614,9 +614,9 @@ void Unicycle3D::validateMotionModel( } } -std::ostream & operator<<(std::ostream & stream, const Unicycle3D & unicycle_3d) +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnidirectional_3d) { - unicycle_3d.print(stream); + omnidirectional_3d.print(stream); return stream; } diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp similarity index 95% rename from fuse_models/src/unicycle_3d_ignition.cpp rename to fuse_models/src/omnidirectional_3d_ignition.cpp index 648c15b5c..d545007d0 100644 --- a/fuse_models/src/unicycle_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -61,12 +61,12 @@ #include // Register this motion model with ROS as a plugin. -PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DIgnition, fuse_core::SensorModel); +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3DIgnition, fuse_core::SensorModel); namespace fuse_models { -Unicycle3DIgnition::Unicycle3DIgnition() +Omnidirectional3DIgnition::Omnidirectional3DIgnition() : fuse_core::AsyncSensorModel(1), started_(false), initial_transaction_sent_(false), @@ -75,7 +75,7 @@ Unicycle3DIgnition::Unicycle3DIgnition() { } -void Unicycle3DIgnition::initialize( +void Omnidirectional3DIgnition::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string & name, fuse_core::TransactionCallback transaction_callback) @@ -84,7 +84,7 @@ void Unicycle3DIgnition::initialize( fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); } -void Unicycle3DIgnition::onInit() +void Omnidirectional3DIgnition::onInit() { logger_ = interfaces_.get_node_logging_interface()->get_logger(); clock_ = interfaces_.get_node_clock_interface()->get_clock(); @@ -113,7 +113,7 @@ void Unicycle3DIgnition::onInit() interfaces_, params_.topic, params_.queue_size, - std::bind(&Unicycle3DIgnition::subscriberCallback, this, std::placeholders::_1), + std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options ); @@ -124,7 +124,7 @@ void Unicycle3DIgnition::onInit() interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), std::bind( - &Unicycle3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, + &Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), cb_group_ @@ -136,14 +136,14 @@ void Unicycle3DIgnition::onInit() interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), std::bind( - &Unicycle3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + &Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), cb_group_ ); } -void Unicycle3DIgnition::start() +void Omnidirectional3DIgnition::start() { started_ = true; @@ -171,12 +171,12 @@ void Unicycle3DIgnition::start() } } -void Unicycle3DIgnition::stop() +void Omnidirectional3DIgnition::stop() { started_ = false; } -void Unicycle3DIgnition::subscriberCallback( +void Omnidirectional3DIgnition::subscriberCallback( const geometry_msgs::msg::PoseWithCovarianceStamped & msg) { try { @@ -186,7 +186,7 @@ void Unicycle3DIgnition::subscriberCallback( } } -bool Unicycle3DIgnition::setPoseServiceCallback( +bool Omnidirectional3DIgnition::setPoseServiceCallback( rclcpp::Service::SharedPtr service, std::shared_ptr request_id, const fuse_msgs::srv::SetPose::Request::SharedPtr req) @@ -209,7 +209,7 @@ bool Unicycle3DIgnition::setPoseServiceCallback( return true; } -bool Unicycle3DIgnition::setPoseDeprecatedServiceCallback( +bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( rclcpp::Service::SharedPtr service, std::shared_ptr request_id, const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) @@ -229,7 +229,7 @@ bool Unicycle3DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Unicycle3DIgnition::process( +void Omnidirectional3DIgnition::process( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) { // Verify we are in the correct state to process set pose requests @@ -329,7 +329,7 @@ void Unicycle3DIgnition::process( } } -void Unicycle3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { const auto & stamp = pose.header.stamp; diff --git a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp similarity index 83% rename from fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp rename to fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 0fe1f3a46..7caf9dc3b 100644 --- a/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -36,8 +36,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -48,7 +48,7 @@ namespace fuse_models { -Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( +Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, const fuse_variables::Orientation3DStamped & orientation1, @@ -78,7 +78,7 @@ Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( { } -void Unicycle3DStateKinematicConstraint::print(std::ostream & stream) const +void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -97,21 +97,21 @@ void Unicycle3DStateKinematicConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * Unicycle3DStateKinematicConstraint::costFunction() const +ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() const { - return new Unicycle3DStateCostFunction(dt_, sqrt_information_); + return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // - // return new ceres::AutoDiffCostFunction( - // new Unicycle3DStateCostFunctor(dt_, sqrt_information_)); + // return new ceres::AutoDiffCostFunction( + // new Omnidirectional3DStateCostFunctor(dt_, sqrt_information_)); // And including the followings: // #include - // #include + // #include } } // namespace fuse_models -BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Unicycle3DStateKinematicConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DStateKinematicConstraint, fuse_core::Constraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Omnidirectional3DStateKinematicConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3DStateKinematicConstraint, fuse_core::Constraint); diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index 15e15aad1..30602e78b 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,9 +5,9 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition - test_unicycle_3d - test_unicycle_3d_predict - test_unicycle_3d_state_cost_function + test_omnidirectional_3d + test_omnidirectional_3d_predict + test_omnidirectional_3d_state_cost_function ) foreach(test_name ${TEST_TARGETS}) diff --git a/fuse_models/test/test_unicycle_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp similarity index 95% rename from fuse_models/test/test_unicycle_3d.cpp rename to fuse_models/test/test_omnidirectional_3d.cpp index b8dfebfa9..f82bb75a2 100644 --- a/fuse_models/test/test_unicycle_3d.cpp +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include @@ -18,15 +18,15 @@ /** * @brief Derived class used in unit tests to expose protected functions */ -class Unicycle3DModelTest : public fuse_models::Unicycle3D +class Omnidirectional3DModelTest : public fuse_models::Omnidirectional3D { public: - using fuse_models::Unicycle3D::updateStateHistoryEstimates; - using fuse_models::Unicycle3D::StateHistoryElement; - using fuse_models::Unicycle3D::StateHistory; + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::StateHistory; }; -TEST(Unicycle3D, UpdateStateHistoryEstimates) +TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // Create some variables auto position1 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0)); @@ -155,10 +155,10 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) graph.addVariable(linear_acceleration4); // Add all of the variables to the state history - Unicycle3DModelTest::StateHistory state_history; + Omnidirectional3DModelTest::StateHistory state_history; state_history.emplace( position1->stamp(), - Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) position1->uuid(), orientation1->uuid(), linear_velocity1->uuid(), @@ -171,7 +171,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) state_history.emplace( position2->stamp(), - Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) position2->uuid(), orientation2->uuid(), linear_velocity2->uuid(), @@ -184,7 +184,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) state_history.emplace( position3->stamp(), - Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) position3->uuid(), orientation3->uuid(), linear_velocity3->uuid(), @@ -197,7 +197,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) state_history.emplace( position4->stamp(), - Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) position4->uuid(), orientation4->uuid(), linear_velocity4->uuid(), @@ -210,7 +210,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) state_history.emplace( position5->stamp(), - Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) position5->uuid(), orientation5->uuid(), linear_velocity5->uuid(), @@ -223,7 +223,7 @@ TEST(Unicycle3D, UpdateStateHistoryEstimates) fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) // Update the state history - Unicycle3DModelTest::updateStateHistoryEstimates( + Omnidirectional3DModelTest::updateStateHistoryEstimates( graph, state_history, rclcpp::Duration::from_seconds( 10.0)); diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp similarity index 99% rename from fuse_models/test/test_unicycle_3d_predict.cpp rename to fuse_models/test/test_omnidirectional_3d_predict.cpp index 0b47a97c9..230c0b8cc 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include TEST(Predict, predictDirectVals) diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp similarity index 93% rename from fuse_models/test/test_unicycle_3d_state_cost_function.cpp rename to fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp index c0ce74508..bea153f97 100644 --- a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -41,8 +41,8 @@ #include #include -#include -#include +#include +#include TEST(CostFunction, evaluateCostFunction) { @@ -55,7 +55,7 @@ TEST(CostFunction, evaluateCostFunction) const double dt{0.1}; const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; - const fuse_models::Unicycle3DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Omnidirectional3DStateCostFunction cost_function{dt, sqrt_information}; // Evaluate cost function const double position1[3] = {0.0, 0.0, 0.0}; @@ -120,8 +120,8 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Unicycle3DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); std::vector jacobians_autodiff(num_parameter_blocks); From 4ee398168df2c8603d2ffd63c10251c68a5a9d8e Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Tue, 16 Jan 2024 10:13:38 +0000 Subject: [PATCH 088/116] update fuse.repos --- fuse.repos | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 fuse.repos diff --git a/fuse.repos b/fuse.repos new file mode 100644 index 000000000..8ad27e25b --- /dev/null +++ b/fuse.repos @@ -0,0 +1,9 @@ +repositories: + locusrobotics/tf2_2d: + type: git + url: https://github.com/locusrobotics/tf2_2d.git + version: rolling + covariance_geometry_ros: + type: git + url: https://github.com/giafranchini/covariance_geometry_ros.git + version: iron \ No newline at end of file From ca7e435317b004c9134cd8dbf7c59b0f6f8f7ec7 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Mon, 29 Jan 2024 10:01:17 +0000 Subject: [PATCH 089/116] feature: covariance variation check in processDifferentialPose3DWithCovariance --- .../fuse_models/common/sensor_proc.hpp | 172 ++++++++++-------- 1 file changed, 93 insertions(+), 79 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index c229a3519..09a503eb8 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -1012,101 +1012,116 @@ inline bool processDifferentialPose3DWithCovariance( const std::vector & orientation_indices, const bool validate, fuse_core::Transaction & transaction) -{ - // Here we are using covariance_geometry types to compute the relative pose covariance: - // PoseQuaternionCovarianceRPY is std::pair, Covariance> - // Position is Eigen::Vector3d - // Quaternion is Eigen::Quaterniond - // Covariance is Eigen::Matrix6d - - // Convert from ROS msg to covariance geometry types - covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; - covariance_geometry::fromROS(pose1.pose, p1); - covariance_geometry::fromROS(pose2.pose, p2); - +{ // Create the pose variables auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); - position1->x() = p1.first.first.x(); - position1->y() = p1.first.first.y(); - position1->z() = p1.first.first.z(); - orientation1->x() = p1.first.second.x(); - orientation1->y() = p1.first.second.y(); - orientation1->z() = p1.first.second.z(); - orientation1->w() = p1.first.second.w(); + position1->x() = pose1.pose.pose.position.x; + position1->y() = pose1.pose.pose.position.y; + position1->z() = pose1.pose.pose.position.z; + orientation1->x() = pose1.pose.pose.orientation.x; + orientation1->y() = pose1.pose.pose.orientation.y; + orientation1->z() = pose1.pose.pose.orientation.z; + orientation1->w() = pose1.pose.pose.orientation.w; auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); - position2->x() = p2.first.first.x(); - position2->y() = p2.first.first.y(); - position2->z() = p2.first.first.z(); - orientation2->x() = p2.first.second.x(); - orientation2->y() = p2.first.second.y(); - orientation2->z() = p2.first.second.z(); - orientation2->w() = p2.first.second.w(); + position2->x() = pose2.pose.pose.position.x; + position2->y() = pose2.pose.pose.position.y; + position2->z() = pose2.pose.pose.position.z; + orientation2->x() = pose2.pose.pose.orientation.x; + orientation2->y() = pose2.pose.pose.orientation.y; + orientation2->z() = pose2.pose.pose.orientation.z; + orientation2->w() = pose2.pose.pose.orientation.w; + + // Here we are using covariance_geometry types to compute the relative pose covariance: + // PoseQuaternionCovarianceRPY is std::pair, Covariance> + // Position is Eigen::Vector3d + // Quaternion is Eigen::Quaterniond + // Covariance is Eigen::Matrix6d + + // Convert from ROS msg to covariance geometry types + covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::fromROS(pose1.pose, p1); + covariance_geometry::fromROS(pose2.pose, p2); // Create the delta for the constraint if (independent) { covariance_geometry::ComposePoseQuaternionCovarianceRPY( - covariance_geometry::inversePose3DQuaternionCovarianceRPY(p1), + covariance_geometry::InversePose3DQuaternionCovarianceRPY(p1), p2, p12 ); } else { - // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. - // We know cov1 and cov2 and we should substract the first to the second in order - // to obtain the relative pose covariance. But first the 2 of them have to be in the - // same reference frame, moreover we need to rotate the result in p12 reference frame - // The covariance propagation of p2 = p1 * p12 is: - // - // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T - // - // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and - // J_p12 are the jacobians of the equation (pose composition) wrt p1 and p12, respectively. - // - // Therefore, the covariance C12 of the relative pose p12 is: - // - // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T - - // First we need to convert covariances from RPY(6x6) to Quat(7x7) - covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; - covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( - p1, - p1_q - ); - covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( - p2, - p2_q - ); - // Then we need to compute the delta pose - covariance_geometry::ComposePose3DQuaternion( - covariance_geometry::InversePose(p1_q.first), - p2_q.first, - p12_q.first - ); - // Now we have to compute pose composition jacobians so we can rotate covariances - Eigen::Matrix7d j_p1; - Eigen::Matrix7d j_p12; - Eigen::Matrix4d j_qn; - j_p1.setZero(); - j_p12.setZero(); - j_qn.setZero(); - covariance_geometry::jacobianQuaternionNormalization(p2_q.first.second, j_qn); - covariance_geometry::JacobianPosePoseCompositionA(p1_q.first, p12_q.first, j_p1); - j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); - covariance_geometry::JacobianPosePoseCompositionB(p1_q.first, j_p12); - j_p12.block<4, 4>(3, 3).applyOnTheLeft(j_qn); - Eigen::Matrix7d j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); - p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * - j_p12_inv.transpose(); - - // Now again convert the delta pose back to covariance in RPY(6x6) - covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( + // If the covariances are the same, then it's possible that the covariance of the relative + // pose will be zero or ill-conditioned. To avoid this, we skip the expensive following + // calculations and instead we just add a minimum covariance later + if (p1.second.isApprox(p2.second, 1e-9)) { + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1.first), + p2.first, + p12.first + ); + p12.second.setZero(); + } else { + // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. + // We know cov1 and cov2 and we should substract the first to the second in order + // to obtain the relative pose covariance. But first the 2 of them have to be in the + // same reference frame, moreover we need to rotate the result in p12 reference frame + // The covariance propagation of p2 = p1 * p12 is: + // + // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T + // + // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and + // J_p12 are the jacobians of the equation (pose composition) wrt p1 and p12, respectively. + // + // Therefore, the covariance C12 of the relative pose p12 is: + // + // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T + + // First we need to convert covariances from RPY (6x6) to quaternion (7x7) + covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p1, + p1_q + ); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p2, + p2_q + ); + // Then we need to compute the delta pose + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1_q.first), + p2_q.first, + p12_q.first + ); + // Now we have to compute pose composition jacobians so we can rotate covariances + Eigen::Matrix7d j_p1, j_p12, j_p12_inv; + Eigen::Matrix4d j_qn; + + covariance_geometry::jacobianQuaternionNormalization(p12_q.first.second, j_qn); + + covariance_geometry::JacobianPosePoseCompositionA(p1_q.first, p12_q.first, j_p1); + j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p1.block<4, 3>(3, 0).setZero(); + + covariance_geometry::JacobianPosePoseCompositionB(p1_q.first, j_p12); + j_p12.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p12.block<3, 4>(0, 3).setZero(); + j_p12.block<4, 3>(3, 0).setZero(); + + // TODO(giafranchini): check if faster to use j12.llt().solve() instead + j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); + p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * j_p12_inv.transpose(); + + // Now again convert the delta pose covariance back to RPY(6x6) + covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( p12_q, p12 - ); + ); + } } if (position_indices.size() == 3 && orientation_indices.size() == 3) { @@ -1155,13 +1170,12 @@ inline bool processDifferentialPose3DWithCovariance( // Convert the poses into RPY representation fuse_core::Vector6d pose_relative_mean_partial; - fuse_core::Matrix6d pose_relative_covariance = p12.second; tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), p12.first.second.w()); pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), p12.first.first.z(); tf2::Matrix3x3(q12).getRPY(pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); - pose_relative_covariance += minimum_pose_relative_covariance; + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; const auto indices = mergeIndices(position_indices, orientation_indices, 3); From 3b36fe4c98c2b37303ea51dcd4d46883e14f1cb1 Mon Sep 17 00:00:00 2001 From: giacomo Date: Thu, 1 Feb 2024 17:42:22 +0100 Subject: [PATCH 090/116] add some comments in sensor_proc --- .../fuse_models/common/sensor_proc.hpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 09a503eb8..260f7e548 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -1036,16 +1036,20 @@ inline bool processDifferentialPose3DWithCovariance( orientation2->z() = pose2.pose.pose.orientation.z; orientation2->w() = pose2.pose.pose.orientation.w; - // Here we are using covariance_geometry types to compute the relative pose covariance: - // PoseQuaternionCovarianceRPY is std::pair, Covariance> - // Position is Eigen::Vector3d - // Quaternion is Eigen::Quaterniond - // Covariance is Eigen::Matrix6d + // Here we are using covariance_geometry types to compute the relative pose covariance: + // PoseQuaternionCovarianceRPY is std::pair, Covariance> + // Position is Eigen::Vector3d + // Quaternion is Eigen::Quaterniond + // Covariance is Eigen::Matrix6d - // Convert from ROS msg to covariance geometry types - covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; - covariance_geometry::fromROS(pose1.pose, p1); - covariance_geometry::fromROS(pose2.pose, p2); + // N.B. covariance_geometry implements functions for pose composition and covariance propagation + // which are based on "A tutorial on SE(3) transformation parameterizations and on-manifold optimization" + // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) + + // Convert from ROS msg to covariance geometry types + covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::fromROS(pose1.pose, p1); + covariance_geometry::fromROS(pose2.pose, p2); // Create the delta for the constraint if (independent) { @@ -1055,9 +1059,9 @@ inline bool processDifferentialPose3DWithCovariance( p12 ); } else { - // If the covariances are the same, then it's possible that the covariance of the relative - // pose will be zero or ill-conditioned. To avoid this, we skip the expensive following - // calculations and instead we just add a minimum covariance later + // If covariances of p1 and p2 are the same, then it's possible that p12 covariance will be + // zero or ill-conditioned. To avoid this, we skip the expensive following calculations and + // instead we just add a minimum covariance later if (p1.second.isApprox(p2.second, 1e-9)) { covariance_geometry::ComposePose3DQuaternion( covariance_geometry::InversePose(p1.first), From 0b44b7ab08fbc9ab8b5b7dfa70f3cf4ff38ed127 Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 11 Mar 2024 14:26:37 +0100 Subject: [PATCH 091/116] revert composable optimizer --- .../fuse_models/common/sensor_proc.hpp | 8 +-- fuse_optimizers/CMakeLists.txt | 34 +++---------- .../batch_optimizer_component.hpp | 20 -------- .../fixed_lag_smoother_component.hpp | 19 ------- .../src/batch_optimizer_component.cpp | 14 ------ fuse_optimizers/src/batch_optimizer_node.cpp | 47 ++++++++++++++++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 3 +- .../src/fixed_lag_smoother_component.cpp | 15 ------ .../src/fixed_lag_smoother_node.cpp | 49 +++++++++++++++++++ 9 files changed, 109 insertions(+), 100 deletions(-) delete mode 100644 fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp delete mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp delete mode 100644 fuse_optimizers/src/batch_optimizer_component.cpp create mode 100644 fuse_optimizers/src/batch_optimizer_node.cpp delete mode 100644 fuse_optimizers/src/fixed_lag_smoother_component.cpp create mode 100644 fuse_optimizers/src/fixed_lag_smoother_node.cpp diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 260f7e548..4a851cd9b 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -1398,7 +1398,7 @@ inline bool processDifferentialPoseWithTwistCovariance( try { validatePartialMeasurement( pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-4); + 1e-6); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -1731,7 +1731,7 @@ inline bool processTwistWithCovariance( if (validate) { try { - validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-4); + validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -1771,7 +1771,7 @@ inline bool processTwistWithCovariance( if (validate) { try { - validatePartialMeasurement(angular_vel_vector, angular_vel_covariance, 1e-4); + validatePartialMeasurement(angular_vel_vector, angular_vel_covariance); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, @@ -2069,7 +2069,7 @@ inline bool processAccelWithCovariance( if (validate) { try { - validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 75fa7219d..6f21b1ce8 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -54,33 +54,13 @@ target_link_libraries(${PROJECT_NAME} ) ament_target_dependencies(${PROJECT_NAME} diagnostic_updater) -## batch_optimizer component -add_library(batch_optimizer_component SHARED src/batch_optimizer_component.cpp) -target_include_directories(batch_optimizer_component PRIVATE include) -ament_target_dependencies( - batch_optimizer_component - SYSTEM - rclcpp_components -) -target_link_libraries(batch_optimizer_component ${PROJECT_NAME}) - -rclcpp_components_register_node( - batch_optimizer_component PLUGIN "fuse_optimizers::BatchOptimizerComponent" EXECUTABLE - batch_optimizer_node) - -## fixed_lag_smoother component -add_library(fixed_lag_smoother_component SHARED src/fixed_lag_smoother_component.cpp) -target_include_directories(fixed_lag_smoother_component PRIVATE include) -ament_target_dependencies( - fixed_lag_smoother_component - SYSTEM - rclcpp_components -) -target_link_libraries(fixed_lag_smoother_component ${PROJECT_NAME}) +## batch_optimizer node +add_executable(batch_optimizer_node src/batch_optimizer_node.cpp) +target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) -rclcpp_components_register_node( - fixed_lag_smoother_component PLUGIN "fuse_optimizers::FixedLagSmootherComponent" EXECUTABLE - fixed_lag_smoother_node) +## fixed_lag_smoother node +add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) +target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) ############# ## Testing ## @@ -96,7 +76,7 @@ endif() ## Install ## ############# -install(TARGETS ${PROJECT_NAME} batch_optimizer_component fixed_lag_smoother_component EXPORT ${PROJECT_NAME}-export +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp deleted file mode 100644 index 37306090c..000000000 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ -#define FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "fuse_optimizers/batch_optimizer.hpp" - -namespace fuse_optimizers -{ - class BatchOptimizerComponent : public rclcpp::Node - { - public: - explicit BatchOptimizerComponent(const rclcpp::NodeOptions& options); - - - private: - std::shared_ptr batch_optimizer_; - }; -} // namespace fuse_optimizers - -#endif // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp deleted file mode 100644 index a27aab423..000000000 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ -#define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ - -#include -#include - -namespace fuse_optimizers -{ - class FixedLagSmootherComponent : public rclcpp::Node - { - public: - explicit FixedLagSmootherComponent(const rclcpp::NodeOptions& options); - - private: - std::shared_ptr fixed_lag_smoother_; - }; -} // namespace fuse_optimizers - -#endif // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/src/batch_optimizer_component.cpp b/fuse_optimizers/src/batch_optimizer_component.cpp deleted file mode 100644 index 113792cfa..000000000 --- a/fuse_optimizers/src/batch_optimizer_component.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "fuse_optimizers/batch_optimizer_component.hpp" -namespace fuse_optimizers -{ - BatchOptimizerComponent::BatchOptimizerComponent(const rclcpp::NodeOptions& options) - : Node("batch_optimizer", options), - batch_optimizer_(std::make_shared(*this)) - { - RCLCPP_INFO(get_logger(), "BatchOptimizerComponent constructed"); - } -} // namespace fuse_optimizers - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizerComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp new file mode 100644 index 000000000..e7d00b85f --- /dev/null +++ b/fuse_optimizers/src/batch_optimizer_node.cpp @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("batch_optimizer_node"); + auto optimizer = std::make_shared(*node); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f80960862..46dd6e7f4 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -253,6 +253,8 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); + // std::cout << summary_.FullReport() << std::endl; + // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); @@ -400,7 +402,6 @@ void FixedLagSmoother::processQueue( return; } } - // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); diff --git a/fuse_optimizers/src/fixed_lag_smoother_component.cpp b/fuse_optimizers/src/fixed_lag_smoother_component.cpp deleted file mode 100644 index 66f650a07..000000000 --- a/fuse_optimizers/src/fixed_lag_smoother_component.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "fuse_optimizers/fixed_lag_smoother_component.hpp" - -namespace fuse_optimizers -{ - FixedLagSmootherComponent::FixedLagSmootherComponent(const rclcpp::NodeOptions& options) - : Node("fixed_lag_smoother", options), - fixed_lag_smoother_(std::make_shared(*this)) - { - RCLCPP_INFO(get_logger(), "FixedLagSmootherComponent constructed"); - } -} // namespace fuse_optimizers - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmootherComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp new file mode 100644 index 000000000..7be160efa --- /dev/null +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -0,0 +1,49 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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. + */ + +#include +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("fixed_lag_smoother_node"); + auto optimizer = std::make_shared(*node); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 88204cd7b3bf615d9bbae1831b34b4a7bbc25a89 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 11 Mar 2024 14:46:12 +0100 Subject: [PATCH 092/116] Update fuse_core/include/fuse_core/util.hpp Co-authored-by: Stephen Williams --- fuse_core/include/fuse_core/util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index 1908eff2a..e2eb35e31 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -239,7 +239,7 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob * * @param[in] z Pointer to the first quaternion array (4x1) * @param[in] w Pointer to the second quaternion array (4x1) - * @param[in] z Pointer to the first quaternion array (4x1) + * @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w * @param[in] jacobian Pointer to the jacobian matrix (4x4, optional) */ static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr) From 0878114073978c6a818b08c397483df35612c9f0 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 11 Mar 2024 14:46:45 +0100 Subject: [PATCH 093/116] Update fuse_core/include/fuse_core/util.hpp Co-authored-by: Stephen Williams --- fuse_core/include/fuse_core/util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index e2eb35e31..b2b9ddc3c 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -240,7 +240,7 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob * @param[in] z Pointer to the first quaternion array (4x1) * @param[in] w Pointer to the second quaternion array (4x1) * @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w - * @param[in] jacobian Pointer to the jacobian matrix (4x4, optional) + * @param[in] jacobian Pointer to the jacobian of zw with respect to w (4x4, optional) */ static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr) { From ee059b622832c42924929d89443f1df25fc0f80a Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 11 Mar 2024 16:46:03 +0100 Subject: [PATCH 094/116] Tests for quat2rpy - quat product - quat2angleaxis jacobians --- fuse_core/test/test_util.cpp | 148 ++++++++++++++++++++++++++++++++++- 1 file changed, 145 insertions(+), 3 deletions(-) diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 7976550f2..1ce51eb79 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -36,7 +36,50 @@ #include #include +#include #include +#include + +struct Quat2RPY { + template + bool operator()(const T * const q, T * rpy) const { + rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); + rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); + rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new Quat2RPY())); + } +}; + +struct QuatProd { + template + bool operator()( + const T * const q1, + const T * const q2, + T * q_out) const { + ceres::QuaternionProduct(q1, q2, q_out); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new QuatProd())); + } +}; + +struct Quat2AngleAxis { + template + bool operator()(const T * const q, T * aa) const { + ceres::QuaternionToAngleAxis(q, aa); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); + } +}; TEST(Util, wrapAngle2D) { @@ -84,6 +127,7 @@ TEST(Util, wrapAngle2D) TEST(Util, quaternion2rpy) { + // Test correct conversion from quaternion to roll-pitch-yaw double q[4] = {1.0, 0.0, 0.0, 0.0}; double rpy[3]; fuse_core::quaternion2rpy(q, rpy); @@ -101,9 +145,107 @@ TEST(Util, quaternion2rpy) EXPECT_NEAR(0.2, rpy[1], 1e-6); EXPECT_NEAR(-0.3, rpy[2], 1e-6); - //TODO(giafranchini): Add test for jacobian + // Test correct quaternion to roll-pitch-yaw function jacobian + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double J_analytic[12], J_autodiff[12]; + q[0] = q_eigen.w(); + q[1] = q_eigen.x(); + q[2] = q_eigen.y(); + q[3] = q_eigen.z(); + + fuse_core::quaternion2rpy(q, rpy, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); + double rpy_autodiff[3]; + quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } -//TODO(giafranchini): Add test for quaternionProduct -//TODO(giafranchini): Add test for quaternionInverse +TEST(Util, quaternionProduct) +{ + // Test correct quaternion product function jacobian + const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); + const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); + double q_out[4]; + double q1[4] + { + q1_eigen.w(), + q1_eigen.x(), + q1_eigen.y(), + q1_eigen.z() + }; + + double q2[4] + { + q2_eigen.w(), + q2_eigen.x(), + q2_eigen.y(), + q2_eigen.z() + }; + + // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + + double * jacobians[2]; + jacobians[0] = J_autodiff_q1; + jacobians[1] = J_autodiff_q2; + + const double * parameters[2]; + parameters[0] = q1; + parameters[1] = q2; + + ceres::CostFunction * quat_prod_cf = QuatProd::Create(); + double q_out_autodiff[4]; + quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); + + Eigen::Map> J_autodiff_q1_map(jacobians[0]); + Eigen::Map> J_autodiff_q2_map(jacobians[1]); + + // Eigen::Map> J_analytic_q1_map(J_analytic_q1); + Eigen::Map> J_analytic_q2_map(J_analytic_q2); + + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); +} + +TEST(Util, quaternionToAngleAxis) +{ + // Test correct quaternion to angle-axis function jacobian + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double angle_axis[3]; + double q[4] + { + q_eigen.w(), + q_eigen.x(), + q_eigen.y(), + q_eigen.z() + }; + + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); +} From 69344a164a050fa6e29c49a83da1c746b9393df8 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:14:37 +0100 Subject: [PATCH 095/116] Remove comments in absolute_pose_3d_stamped_constraint.cpp --- .../src/absolute_pose_3d_stamped_constraint.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index ca248c9f0..324b7fbc6 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -75,16 +75,6 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const { return new NormalPriorPose3D(sqrt_information_, mean_); - - // Here we return a cost function that computes the analytic derivatives/jacobians, but we could - // use automatic differentiation as follows: - - // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DCostFunctor(sqrt_information_, mean_); - - // And including the followings: - // #include - // #include } } // namespace fuse_constraints From 8a5b97cdf6e8598cd14de3024215b4ebb192d141 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:30:37 +0100 Subject: [PATCH 096/116] Add TODOs normal_prior_pose_3d.cpp --- fuse_constraints/src/normal_prior_pose_3d.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 47eaf11bd..581e455dd 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -76,7 +76,10 @@ bool NormalPriorPose3D::Evaluate( residuals[0] = parameters[0][0] - b_[0]; residuals[1] = parameters[0][1] - b_[1]; residuals[2] = parameters[0][2] - b_[2]; - // Compute orientation residuals + // Compute orientation residuals + + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis From a8c4d7bd621d6a14cd26d7bebe1b110d5ac536ab Mon Sep 17 00:00:00 2001 From: Giacomo Franchini <83601983+giafranchini@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:32:46 +0100 Subject: [PATCH 097/116] Add TODO to normal_prior_orientation_3d.cpp --- fuse_constraints/src/normal_prior_orientation_3d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index de79fa767..f1181bab3 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -71,6 +71,7 @@ bool NormalPriorOrientation3D::Evaluate( double j_product[16]; double j_quat2angle[12]; + // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis From cba0a03087a526fbf72fc68aa1ff3683d84b10c5 Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 11 Mar 2024 17:35:28 +0100 Subject: [PATCH 098/116] Add single jacobian check in normal_prior_orientation_3d --- fuse_constraints/src/normal_prior_orientation_3d.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index f1181bab3..34269cedc 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -81,10 +81,12 @@ bool NormalPriorOrientation3D::Evaluate( residuals_map.applyOnTheLeft(A_); if (jacobians != nullptr) { - Eigen::Map> j_map(jacobians[0]); - Eigen::Map j_product_map(j_product); - Eigen::Map> j_quat2angle_map(j_quat2angle); - j_map = A_ * j_quat2angle_map * j_product_map; + if (jacobians[0] != nullptr) { + Eigen::Map> j_map(jacobians[0]); + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + j_map = A_ * j_quat2angle_map * j_product_map; + } } return true; } From 90ded4ba333b426ad56d25eddf575c9b6027dfa7 Mon Sep 17 00:00:00 2001 From: giacomo Date: Mon, 11 Mar 2024 18:50:54 +0100 Subject: [PATCH 099/116] Formalize comparisons in quaternion2rpy --- fuse_core/include/fuse_core/util.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b2b9ddc3c..5766b349e 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -171,21 +171,23 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob const double qy = q[2]; const double qz = q[3]; const double discr = qw * qy - qx * qz; - jacobian_map.setZero(); + const double gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); - if (discr > 0.49999) { + if (discr > gl_limit) { // pitch = 90 deg - jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map.setZero(); + jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; - } else if (discr < -0.49999) { + } else if (discr < -gl_limit) { // pitch = -90 deg - jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map.setZero(); + jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; } else { // Non-degenerate case: - jacobian_map(0, 0) = + jacobian_map(0, 0) = -(2.0 * qx) / ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * From 037605096c0a5fe1e56300d80bc8953bbf70a683 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 25 Sep 2024 17:43:29 +0200 Subject: [PATCH 100/116] Update fuse_models/include/fuse_models/omnidirectional_3d.hpp Co-authored-by: Henry Moore --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index f8e4ccab3..20ac8c296 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -237,7 +237,7 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & unicycle_2d); +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnidirectional_3d); } // namespace fuse_models From 05bc6a3e80065cf24388caa425f07b198a135994 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 25 Sep 2024 17:43:47 +0200 Subject: [PATCH 101/116] Update fuse_models/include/fuse_models/omnidirectional_3d.hpp Co-authored-by: Henry Moore --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 20ac8c296..2490698ae 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -241,4 +241,4 @@ std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnid } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ From e729204c588c4bfca6d22c3687ec42f2b5a41b69 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 25 Sep 2024 17:44:02 +0200 Subject: [PATCH 102/116] Update fuse_models/include/fuse_models/omnidirectional_3d.hpp Co-authored-by: Henry Moore --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 2490698ae..6089c92fa 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ #include #include From 80aec70f29feae973f18fe544be6a02e4bac9ba1 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 25 Sep 2024 17:44:15 +0200 Subject: [PATCH 103/116] Update fuse_models/include/fuse_models/omnidirectional_3d.hpp Co-authored-by: Henry Moore --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 6089c92fa..c999b75b2 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -36,7 +36,6 @@ #include #include -#include #include #include From d26d91f1979d950836c7f38f4b5eb21fd0553253 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 25 Sep 2024 16:07:08 +0000 Subject: [PATCH 104/116] fix typos --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 2 +- .../include/fuse_models/omnidirectional_3d_ignition.hpp | 8 ++++---- .../include/fuse_models/omnidirectional_3d_predict.hpp | 6 +++--- .../omnidirectional_3d_state_cost_function.hpp | 6 +++--- .../fuse_models/omnidirectional_3d_state_cost_functor.hpp | 6 +++--- .../omnidirectional_3d_state_kinematic_constraint.hpp | 6 +++--- .../parameters/omnidirectional_3d_ignition_params.hpp | 6 +++--- 7 files changed, 20 insertions(+), 20 deletions(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index c999b75b2..f5081096f 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -55,7 +55,7 @@ namespace fuse_models * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided * time stamps, and adds those constraints to the fuse graph. * - * This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model + * This class uses a omnidirectional kinematic model for the robot. It is equivalent to the motion model * in the robot_localization state estimation nodes. * * Parameters: diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index ecbf39028..1f400fdbc 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ #include #include @@ -58,7 +58,7 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this @@ -217,4 +217,4 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 1a9a3501d..08a1b9007 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ #include @@ -644,4 +644,4 @@ inline void predict( } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 730d48def..49349a0e9 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ #include @@ -289,4 +289,4 @@ Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction( } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index d95523a01..86a7827b4 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ #include @@ -214,4 +214,4 @@ bool Omnidirectional3DStateCostFunctor::operator()( } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index a61de7bab..f196c55bc 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ #include #include @@ -185,4 +185,4 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint BOOST_CLASS_EXPORT_KEY(fuse_models::Omnidirectional3DStateKinematicConstraint); -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index 053ac5572..acb1fd286 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ -#define FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#ifndef FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ #include #include @@ -215,4 +215,4 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase } // namespace fuse_models -#endif // FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#endif // FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ From 46899303808526608f871e2cd47f25604951abc7 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Wed, 25 Sep 2024 16:11:43 +0000 Subject: [PATCH 105/116] fix typo: quat instead of rpy --- .../include/fuse_models/omnidirectional_3d_ignition.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index 1f400fdbc..bb20170cf 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -58,7 +58,7 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this @@ -78,7 +78,7 @@ namespace fuse_models * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for * the state. Variable order is (x, y, z, - * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before From 05a35c23b6b524090a0b09ec698970e4628de540 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Thu, 26 Sep 2024 17:10:51 +0200 Subject: [PATCH 106/116] Fix wrong stride in linear acceleration covariance eigen map Co-authored-by: Henry Moore --- fuse_models/include/fuse_models/common/sensor_proc.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 4a851cd9b..79886f829 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -2160,7 +2160,7 @@ inline bool processAccel3DWithCovariance( transformed_message.accel.accel.linear.y, transformed_message.accel.accel.linear.z; - Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); + Eigen::Map> accel_covariance_map(transformed_message.accel.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); From 0e646761e845947160e255602e673648e29f32b2 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 3 Oct 2024 14:44:22 +0000 Subject: [PATCH 107/116] fix omnidirectional_3d_ignition orientation init + tests --- .../src/omnidirectional_3d_ignition.cpp | 3 +- fuse_models/test/CMakeLists.txt | 1 + .../test/test_omnidirectional_3d_ignition.cpp | 474 ++++++++++++++++++ 3 files changed, 476 insertions(+), 2 deletions(-) create mode 100644 fuse_models/test/test_omnidirectional_3d_ignition.cpp diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index d545007d0..519f45e1d 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -153,8 +153,7 @@ void Omnidirectional3DIgnition::start() if (params_.publish_on_startup && !initial_transaction_sent_) { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); tf2::Quaternion q; - // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); - q.setEuler(params_.initial_state[5], params_.initial_state[4], params_.initial_state[3]); + q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index 30602e78b..048e96778 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -8,6 +8,7 @@ set(TEST_TARGETS test_omnidirectional_3d test_omnidirectional_3d_predict test_omnidirectional_3d_state_cost_function + test_omnidirectional_3d_ignition ) foreach(test_name ${TEST_TARGETS}) diff --git a/fuse_models/test/test_omnidirectional_3d_ignition.cpp b/fuse_models/test/test_omnidirectional_3d_ignition.cpp new file mode 100644 index 000000000..06ac28812 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_ignition.cpp @@ -0,0 +1,474 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Giacomo Franchini + * 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 +#include +#include +#include +#include + +using fuse_constraints::AbsolutePosition3DStampedConstraint; +using fuse_constraints::AbsoluteOrientation3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint; +using fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint; + + +/** + * @brief Promise used to communicate between the tests and the callback + */ +std::promise callback_promise; + +/** + * @brief Transaction callback that forwards the transaction into the promise result + */ +void transactionCallback(fuse_core::Transaction::SharedPtr transaction) +{ + callback_promise.set_value(std::move(transaction)); +} + +/** + * @brief Helper function for fetching the desired constraint from a transaction + */ +template +const Derived * getConstraint(const fuse_core::Transaction & transaction) +{ + for (const auto & constraint : transaction.addedConstraints()) { + auto derived = dynamic_cast(&constraint); + if (derived) { + return derived; + } + } + return nullptr; +} + + +class Omnidirectional3DIgnitionTestFixture : public ::testing::Test +{ +public: + Omnidirectional3DIgnitionTestFixture() + { + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + rclcpp::shutdown(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(Omnidirectional3DIgnitionTestFixture, InitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 1.2, 2.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 9.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.2, 0.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 16.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 36.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-9); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-9); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SkipInitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_FALSE(status == std::future_status::ready); +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = node->create_client("/omnidirectional_3d_ignition_test/set_pose"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = node->create_client( + "/omnidirectional_3d_ignition_test/set_pose_deprecated"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); // not high precision + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} From 7475196d9f7ed40a38378320ff44ced6522ef035 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 11:28:28 -0600 Subject: [PATCH 108/116] add 3D tutorial (#27) --- fuse_tutorials/CMakeLists.txt | 6 +- fuse_tutorials/config/fuse_3d_tutorial.rviz | 213 ++++++++++ fuse_tutorials/config/fuse_3d_tutorial.yaml | 61 +++ .../launch/fuse_3d_tutorial.launch.py | 67 ++++ .../src/three_dimensional_simulator.cpp | 366 ++++++++++++++++++ 5 files changed, 712 insertions(+), 1 deletion(-) create mode 100644 fuse_tutorials/config/fuse_3d_tutorial.rviz create mode 100644 fuse_tutorials/config/fuse_3d_tutorial.yaml create mode 100755 fuse_tutorials/launch/fuse_3d_tutorial.launch.py create mode 100644 fuse_tutorials/src/three_dimensional_simulator.cpp diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 8dd86a0da..d053b65fc 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -44,10 +44,13 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${sensor_msgs_TARGETS} ) -# tutorial_sim executable +# tutorial_sim executables add_executable(range_sensor_simulator src/range_sensor_simulator.cpp) target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) +add_executable(three_dimensional_simulator src/three_dimensional_simulator.cpp) +target_link_libraries(three_dimensional_simulator ${PROJECT_NAME}) + ############# ## Testing ## ############# @@ -75,6 +78,7 @@ install(TARGETS TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS three_dimensional_simulator DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME} diff --git a/fuse_tutorials/config/fuse_3d_tutorial.rviz b/fuse_tutorials/config/fuse_3d_tutorial.rviz new file mode 100644 index 000000000..863814ccc --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.rviz @@ -0,0 +1,213 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1/Shape1 + - /Odometry2/Shape1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 0; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 0; 255; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_filtered + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10.72309398651123 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml new file mode 100644 index 000000000..59073b3bd --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -0,0 +1,61 @@ +state_estimator: + ros__parameters: + # Fixed-lag smoother configuration + optimization_frequency: 20.0 + transaction_timeout: 0.01 + lag_duration: 0.5 + + # all our sensors will be using this motion model + motion_models: + 3d_motion_model: + type: fuse_models::Omnidirectional3D + + 3d_motion_model: + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + sensor_models: + initial_localization_sensor: + type: fuse_models::Omnidirectional3DIgnition + motion_models: [3d_motion_model] + ignition: true + odometry_sensor: + type: fuse_models::Odometry3D + motion_models: [3d_motion_model] + imu_sensor: + type: fuse_models::Imu3D + motion_models: [3d_motion_model] + + initial_localization_sensor: + publish_on_startup: true + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + odometry_sensor: + topic: 'odom' + twist_target_frame: 'base_link' + # we only want position and orientation, but you can get a full odometry measurement from this sensor + position_dimensions: ['x', 'y', 'z'] + orientation_dimensions: ['roll', 'pitch', 'yaw'] + + imu_sensor: + topic: 'imu' + acceleration_target_frame: 'base_link' + # we only care about linear acceleration for this tutorial + linear_acceleration_dimensions: ['x', 'y', 'z'] + + # this publishes our estimated odometry + publishers: + filtered_publisher: + type: fuse_models::Odometry3DPublisher + + # the configuration of our output publisher + filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'odom' + publish_tf: true + publish_frequency: 10.0 diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py new file mode 100755 index 000000000..f57d66dc1 --- /dev/null +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python3 + +# Copyright 2024 PickNik Robotics +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_dir = FindPackageShare("fuse_tutorials") + + return LaunchDescription( + [ + # tell tf2 that map is the same as odom + # without this, visualization won't work as we have no reference + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], + ), + # run our simulator + Node( + package="fuse_tutorials", + executable="three_dimensional_simulator", + name="three_dimensional_simulator", + output="screen", + ), + # run our estimator + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) + ], + ), + # run visualization + Node( + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + ) + ], + ], + ), + ] + ) diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp new file mode 100644 index 000000000..7bd933208 --- /dev/null +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -0,0 +1,366 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik 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. + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise +static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel +static constexpr double ODOM_POSITION_SIGMA = 0.5; //!< Std dev of simulated odom position measurement noise +static constexpr double ODOM_ORIENTATION_SIGMA = 0.1; //!< Std dev of simulated odom orientation measurement noise + +/** + * @brief The true pose and velocity of the robot + */ +struct Robot +{ + rclcpp::Time stamp; + + double mass; + + double x = 0; + double y = 0; + double z = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + double vx = 0; + double vy = 0; + double vz = 0; + double vroll = 0; + double vpitch = 0; + double vyaw = 0; + double ax = 0; + double ay = 0; + double az = 0; +}; + +/** + * @brief Convert the robot state into a ground truth odometry message + */ +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) +{ + auto msg = std::make_shared(); + msg->header.stamp = state.stamp; + msg->header.frame_id = MAP_FRAME; + msg->child_frame_id = BASELINK_FRAME; + msg->pose.pose.position.x = state.x; + msg->pose.pose.position.y = state.y; + msg->pose.pose.position.z = state.z; + + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->twist.twist.linear.x = state.vx; + msg->twist.twist.linear.y = state.vy; + msg->twist.twist.linear.z = state.vz; + msg->twist.twist.angular.x = state.vroll; + msg->twist.twist.angular.y = state.vpitch; + msg->twist.twist.angular.z = state.vyaw; + + // set covariances + msg->pose.covariance[0] = 0.1; + msg->pose.covariance[7] = 0.1; + msg->pose.covariance[14] = 0.1; + msg->pose.covariance[21] = 0.1; + msg->pose.covariance[28] = 0.1; + msg->pose.covariance[35] = 0.1; + msg->twist.covariance[0] = 0.1; + msg->twist.covariance[7] = 0.1; + msg->twist.covariance[14] = 0.1; + msg->twist.covariance[21] = 0.1; + msg->twist.covariance[28] = 0.1; + msg->twist.covariance[35] = 0.1; + return msg; +} + +/** + * @brief Compute the next robot state given the current robot state and a simulated step time + */ +Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, Eigen::Vector3d external_force) +{ + auto dt = (now - previous_state.stamp).seconds(); + auto next_state = Robot(); + next_state.stamp = now; + next_state.mass = previous_state.mass; + + // just euler integrate to get the next position and velocity + next_state.x = previous_state.x + previous_state.vx * dt; + next_state.y = previous_state.y + previous_state.vy * dt; + next_state.z = previous_state.z + previous_state.vz * dt; + next_state.vx = previous_state.vx + previous_state.ax * dt; + next_state.vy = previous_state.vy + previous_state.ay * dt; + next_state.vz = previous_state.vz + previous_state.az * dt; + + // let's not deal with 3D orientation dynamics for this tutorial + next_state.roll = 0; + next_state.pitch = 0; + next_state.yaw = 0; + next_state.vroll = 0; + next_state.vpitch = 0; + next_state.vyaw = 0; + + // get the current acceleration from the current applied force + next_state.ax = external_force.x() / next_state.mass; + next_state.ay = external_force.y() / next_state.mass; + next_state.az = external_force.z() / next_state.mass; + + return next_state; +} + +/** + * @brief Create a simulated Imu measurement from the current state + */ +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = BASELINK_FRAME; + + // measure accel + msg->linear_acceleration.x = robot.ax + noise(generator); + msg->linear_acceleration.y = robot.ay + noise(generator); + msg->linear_acceleration.z = robot.az + noise(generator); + msg->linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + + // Simulated IMU does not provide orientation + msg->orientation_covariance[0] = -1; + msg->orientation_covariance[4] = -1; + msg->orientation_covariance[8] = -1; + + msg->angular_velocity.x = robot.vroll + noise(generator); + msg->angular_velocity.y = robot.vpitch + noise(generator); + msg->angular_velocity.z = robot.vyaw + noise(generator); + msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + return msg; +} + +nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = ODOM_FRAME; + msg->child_frame_id = BASELINK_FRAME; + + // noisy position measurement + msg->pose.pose.position.x = robot.x + position_noise(generator); + msg->pose.pose.position.y = robot.y + position_noise(generator); + msg->pose.pose.position.z = robot.z + position_noise(generator); + msg->pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + + // noisy orientation measurement + tf2::Quaternion q; + q.setEuler(robot.yaw, robot.pitch, robot.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + return msg; +} + +void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) +{ + // Send the initial localization signal to the state estimator + auto srv = std::make_shared(); + srv->pose.header.frame_id = MAP_FRAME; + srv->pose.pose.pose.position.x = state.x; + srv->pose.pose.pose.position.y = state.y; + srv->pose.pose.pose.position.z = state.z; + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 1.0; + srv->pose.pose.covariance[14] = 1.0; + srv->pose.pose.covariance[21] = 1.0; + srv->pose.pose.covariance[28] = 1.0; + srv->pose.pose.covariance[35] = 1.0; + + auto client = rclcpp::create_client( + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + + while (!client->wait_for_service(std::chrono::seconds(30)) && + interfaces.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + } + + auto success = false; + while (!success) + { + clock->sleep_for(std::chrono::milliseconds(100)); + srv->pose.header.stamp = clock->now(); + auto result_future = client->async_send_request(srv); + + if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(logger, "service call failed :("); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + +int main(int argc, char** argv) +{ + // set up our ROS node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("three_dimensional_simulator"); + + // create our sensor publishers + auto imu_publisher = node->create_publisher("imu", 1); + auto odom_publisher = node->create_publisher("odom", 1); + + // create the ground truth publisher + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + + // Initialize the robot state (state variables are zero-initialized) + auto state = Robot(); + state.stamp = node->now(); + state.mass = 10; // kg + + // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of + // integration inaccuracy on the ground truth + auto rate = rclcpp::Rate(100.0); + + // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, + // which takes care of that. + + // parameters that control the motion pattern of the robot + double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds + double const N_cycles = 2; // number of oscillations per `motion_duration` + + while (rclcpp::ok()) + { + // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) + static auto const first_time = node->now(); + auto const now = node->now(); + + // compensate for the original time offset + double now_d = (now - first_time).seconds(); + // store how long it has been (resetting at `motion_duration` seconds) + double mod_time = std::fmod(now_d, motion_duration); + + // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) + double const force_magnitude = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); + Eigen::Vector3d external_force = { 0, 0, 0 }; + + // switch oscillation axes every `motion_duration` seconds (with one 'rest period') + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) + { + external_force.x() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) + { + external_force.y() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) + { + external_force.z() = force_magnitude; + } + else + { + // reset the robot's position and velocity, leave the external force as 0 + // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) + state.x = 0; + state.y = 0; + state.z = 0; + state.vx = 0; + state.vy = 0; + state.vz = 0; + } + + // Simulate the robot motion + auto new_state = simulateRobotMotion(state, now, external_force); + + // Publish the new ground truth + ground_truth_publisher->publish(*robotToOdometry(new_state)); + + // Generate and publish simulated measurements from the new robot state + imu_publisher->publish(*simulateImu(new_state)); + odom_publisher->publish(*simulateOdometry(new_state)); + + // Wait for the next time step + state = new_state; + rclcpp::spin_some(node); + rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} From ec62d4249912dbcb2e7126a43b39c2e810ebf583 Mon Sep 17 00:00:00 2001 From: giacomo franchini Date: Thu, 3 Oct 2024 17:32:03 +0000 Subject: [PATCH 109/116] Fix wrong quaternion2angleaxis jacobian + test --- fuse_core/include/fuse_core/util.hpp | 6 +-- fuse_core/test/test_util.cpp | 73 +++++++++++++++++++--------- 2 files changed, 52 insertions(+), 27 deletions(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index 5766b349e..c404fc767 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -318,9 +318,9 @@ static inline void quaternionToAngleAxis(const double * q, double * angle_axis, (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); } else { jacobian_map.setZero(); - jacobian_map(1, 1) = 2.0; - jacobian_map(2, 2) = 2.0; - jacobian_map(3, 3) = 2.0; + jacobian_map(0, 1) = 2.0; + jacobian_map(1, 2) = 2.0; + jacobian_map(2, 3) = 2.0; } } } diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 1ce51eb79..69ed76dab 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2024, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -220,32 +221,56 @@ TEST(Util, quaternionProduct) TEST(Util, quaternionToAngleAxis) { - // Test correct quaternion to angle-axis function jacobian - const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); - double angle_axis[3]; - double q[4] + // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation { - q_eigen.w(), - q_eigen.x(), - q_eigen.y(), - q_eigen.z() - }; - - double J_analytic[12]; - double J_autodiff[12]; + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double angle_axis[3]; + double q[4] + { + q_eigen.w(), + q_eigen.x(), + q_eigen.y(), + q_eigen.z() + }; - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); - double * jacobians[1] = {J_autodiff}; - const double * parameters[1] = {q}; + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; - ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); - double angle_axis_autodiff[3]; - quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - - Eigen::Map> J_autodiff_map(jacobians[0]); - Eigen::Map> J_analytic_map(J_analytic); - - EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); -} + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } + + // Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation + { + double angle_axis[3]; + double q[4] {1.0, 0.0, 0.0, 0.0}; + + double J_analytic[12]; + double J_autodiff[12]; + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } +} From 491fda4a2d962d2d666f68a2aa83950ce6b6bfee Mon Sep 17 00:00:00 2001 From: giacomo Date: Thu, 17 Oct 2024 17:09:05 +0200 Subject: [PATCH 110/116] remove summary print --- fuse_optimizers/src/fixed_lag_smoother.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 46dd6e7f4..f8d85f09a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -253,8 +253,6 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); - // std::cout << summary_.FullReport() << std::endl; - // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); From e27f6f8485205ece90f89db53dc2c78d5f83b099 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sat, 20 Apr 2024 09:55:37 -0700 Subject: [PATCH 111/116] Port support for Ceres 2.1.0 Manifold class into ROS 2 Rolling (#366) * Support gcc12 and ceres 2.1.0 * Add support for the Manifold class when using Ceres Solver version 2.1.0 and above * General clean up for Ceres 2.2.0 support * Updated serialization support to be backwards compatible with previously serialized files * Formatting changes required for ROS 2 Rolling / Ubuntu Noble --- fuse_constraints/CMakeLists.txt | 42 +- .../fuse_constraints/marginal_constraint.hpp | 126 +++++- .../marginal_cost_function.hpp | 25 +- .../marginalize_variables.hpp | 3 +- fuse_constraints/src/marginal_constraint.cpp | 5 + .../src/marginal_cost_function.cpp | 34 +- .../src/marginalize_variables.cpp | 32 +- fuse_constraints/src/variable_constraints.cpp | 6 +- fuse_constraints/test/CMakeLists.txt | 3 +- fuse_constraints/test/cost_function_gtest.hpp | 10 +- .../test/legacy_marginal_version0.txt | 3 + .../test/test_absolute_constraint.cpp | 17 + ...lute_orientation_2d_stamped_constraint.cpp | 398 ++++++++++++++++++ ...lute_orientation_3d_stamped_constraint.cpp | 5 + ...rientation_3d_stamped_euler_constraint.cpp | 9 + ...st_absolute_pose_2d_stamped_constraint.cpp | 18 +- ...st_absolute_pose_3d_stamped_constraint.cpp | 9 + .../test/test_marginal_constraint.cpp | 78 +++- .../test/test_relative_constraint.cpp | 25 ++ ...st_relative_pose_2d_stamped_constraint.cpp | 33 ++ ...st_relative_pose_3d_stamped_constraint.cpp | 17 + fuse_core/CMakeLists.txt | 1 + .../include/fuse_core/callback_wrapper.hpp | 2 +- fuse_core/include/fuse_core/ceres_macros.hpp | 2 + fuse_core/include/fuse_core/ceres_options.hpp | 28 +- fuse_core/include/fuse_core/constraint.hpp | 78 ++-- fuse_core/include/fuse_core/eigen_gtest.hpp | 34 +- fuse_core/include/fuse_core/fuse_macros.hpp | 92 ++-- fuse_core/include/fuse_core/graph.hpp | 60 +-- .../fuse_core/local_parameterization.hpp | 128 +++++- fuse_core/include/fuse_core/loss.hpp | 70 +-- fuse_core/include/fuse_core/macros.hpp | 92 ++-- fuse_core/include/fuse_core/manifold.h | 41 ++ fuse_core/include/fuse_core/manifold.hpp | 178 ++++++++ .../include/fuse_core/manifold_adapter.h | 42 ++ .../include/fuse_core/manifold_adapter.hpp | 200 +++++++++ .../include/fuse_core/message_buffer_impl.hpp | 18 +- .../node_interfaces/node_interfaces.hpp | 41 +- fuse_core/include/fuse_core/variable.hpp | 114 +++-- fuse_core/src/ceres_options.cpp | 63 ++- fuse_core/src/manifold_adapter.cpp | 41 ++ fuse_core/src/transaction.cpp | 24 +- fuse_core/test/CMakeLists.txt | 2 +- fuse_core/test/example_constraint.hpp | 1 + fuse_core/test/example_variable.hpp | 155 ++++++- .../test/launch_tests/test_parameters.py | 2 +- .../test/legacy_variable_deserialization.txt | 2 + .../test/test_local_parameterization.cpp | 44 +- fuse_core/test/test_variable.cpp | 161 ++++++- fuse_graphs/CMakeLists.txt | 22 + .../include/fuse_graphs/hash_graph.hpp | 9 + fuse_graphs/src/hash_graph.cpp | 15 +- .../unicycle_2d_ignition_params.hpp | 12 +- .../test_unicycle_2d_state_cost_function.cpp | 6 - fuse_optimizers/src/fixed_lag_smoother.cpp | 24 +- fuse_optimizers/src/optimizer.cpp | 7 +- fuse_optimizers/src/variable_stamp_index.cpp | 6 +- .../launch_tests/test_fixed_lag_ignition.cpp | 2 +- .../launch_tests/test_fixed_lag_ignition.py | 4 +- .../test/launch_tests/test_optimizer.py | 2 +- fuse_publishers/src/path_2d_publisher.cpp | 3 +- .../launch/fuse_simple_tutorial.launch.py | 5 +- .../launch/range_sensor_tutorial.launch.py | 9 +- .../acceleration_angular_2d_stamped.hpp | 13 +- .../acceleration_angular_3d_stamped.hpp | 13 +- .../acceleration_linear_2d_stamped.hpp | 13 +- .../acceleration_linear_3d_stamped.hpp | 13 +- .../fuse_variables/orientation_2d_stamped.hpp | 100 ++++- .../fuse_variables/orientation_3d_stamped.hpp | 149 +++++-- .../point_2d_fixed_landmark.hpp | 11 + .../fuse_variables/point_2d_landmark.hpp | 11 + .../point_3d_fixed_landmark.hpp | 11 + .../fuse_variables/point_3d_landmark.hpp | 11 + .../fuse_variables/position_2d_stamped.hpp | 13 +- .../fuse_variables/position_3d_stamped.hpp | 13 +- .../velocity_angular_2d_stamped.hpp | 13 +- .../velocity_angular_3d_stamped.hpp | 13 +- .../velocity_linear_2d_stamped.hpp | 13 +- .../velocity_linear_3d_stamped.hpp | 13 +- fuse_variables/src/orientation_2d_stamped.cpp | 11 + fuse_variables/src/orientation_3d_stamped.cpp | 11 + .../test_acceleration_angular_2d_stamped.cpp | 3 +- .../test_acceleration_angular_3d_stamped.cpp | 4 +- .../test_acceleration_linear_2d_stamped.cpp | 3 +- .../test_acceleration_linear_3d_stamped.cpp | 3 +- .../test/test_orientation_2d_stamped.cpp | 137 +++++- .../test/test_orientation_3d_stamped.cpp | 218 +++++++++- .../test/test_point_2d_fixed_landmark.cpp | 1 - .../test/test_position_2d_stamped.cpp | 2 +- .../test/test_position_3d_stamped.cpp | 4 +- .../test/test_velocity_angular_2d_stamped.cpp | 2 +- .../test/test_velocity_angular_3d_stamped.cpp | 5 +- .../test/test_velocity_linear_2d_stamped.cpp | 5 +- .../test/test_velocity_linear_3d_stamped.cpp | 5 +- fuse_viz/CMakeLists.txt | 3 +- ...ve_pose_2d_stamped_constraint_property.cpp | 3 +- 96 files changed, 3060 insertions(+), 523 deletions(-) create mode 100644 fuse_constraints/test/legacy_marginal_version0.txt create mode 100644 fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp create mode 100644 fuse_core/include/fuse_core/manifold.h create mode 100644 fuse_core/include/fuse_core/manifold.hpp create mode 100644 fuse_core/include/fuse_core/manifold_adapter.h create mode 100644 fuse_core/include/fuse_core/manifold_adapter.hpp create mode 100644 fuse_core/src/manifold_adapter.cpp create mode 100644 fuse_core/test/legacy_variable_deserialization.txt diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index f6eb3a5d3..2031533ee 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -29,6 +29,47 @@ include(suitesparse-extras.cmake) ########### ## Build ## ########### +# lint_cmake: -linelength +# Disable warnings about array bounds with -Wno-array-bounds until gcc 12 fixes this bug: +# https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247 +# +# Also reported in Eigen, and confirmed to be due to gcc 12: +# https://gitlab.com/libeigen/eigen/-/issues/2506 +# +# In file included from include/immintrin.h:43, +# from include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:361, +# from include/eigen3/Eigen/Core:22, +# from include/fuse_core/fuse_macros.h:63, +# from include/fuse_core/loss.h:37, +# from include/fuse_core/constraint.h:37, +# from src/fuse/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h:37, +# from src/fuse/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp:34: +# In function ‘__m256d _mm256_loadu_pd(const double*)’, +# inlined from ‘Packet Eigen::internal::ploadu(const typename unpacket_traits::type*) [with Packet = __vector(4) double]’ at include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:582:129, +# inlined from ‘Packet Eigen::internal::ploadt(const typename unpacket_traits::type*) [with Packet = __vector(4) double; int Alignment = 0]’ at include/eigen3/Eigen/src/Core/GenericPacketMath.h:969:26, +# inlined from ‘PacketType Eigen::internal::evaluator >::packet(Eigen::Index) const [with int LoadMode = 0; PacketType = __vector(4) double; Derived = Eigen::Matrix]’ at include/eigen3/Eigen/src/Core/CoreEvaluators.h:245:40, +# inlined from ‘void Eigen::internal::generic_dense_assignment_kernel::assignPacket(Eigen::Index) [with int StoreMode = 32; int LoadMode = 0; PacketType = __vector(4) double; DstEvaluatorTypeT = Eigen::internal::evaluator > >; SrcEvaluatorTypeT = Eigen::internal::evaluator >; Functor = Eigen::internal::assign_op; int Version = 0]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:681:114, +# inlined from ‘static void Eigen::internal::dense_assignment_loop::run(Kernel&) [with Kernel = Eigen::internal::generic_dense_assignment_kernel > >, Eigen::internal::evaluator >, Eigen::internal::assign_op, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:437:75, +# inlined from ‘void Eigen::internal::call_dense_assignment_loop(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map >; SrcXprType = Eigen::Matrix; Functor = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:785:37, +# inlined from ‘static void Eigen::internal::Assignment::run(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map >; SrcXprType = Eigen::Matrix; Functor = Eigen::internal::assign_op; Weak = void]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:954:31, +# inlined from ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Map >; Src = Eigen::Matrix; Func = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:890:49, +# inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename enable_if::value, void*>::type) [with Dst = Eigen::Map >; Src = Eigen::Product, Eigen::Map >, 0>; Func = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:851:27, +# inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = Eigen::Map >; Src = Eigen::Product, Eigen::Map >, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:836:18, +# inlined from ‘Derived& Eigen::MatrixBase::operator=(const Eigen::DenseBase&) [with OtherDerived = Eigen::Product, Eigen::Map >, 0>; Derived = Eigen::Map >]’ at include/eigen3/Eigen/src/Core/Assign.h:66:28, +# inlined from ‘void Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map >; Derived = Eigen::Matrix]’ at include/eigen3/Eigen/src/Core/EigenBase.h:114:9: +# include/avxintrin.h:893:24: error: array subscript ‘__m256d_u[0]’ is partly outside array bounds of ‘Eigen::internal::plain_matrix_type, Eigen::Map >, 0>, Eigen::Dense>::type [1]’ {aka ‘Eigen::Matrix [1]’} [-Werror=array-bounds] +# 893 | return *(__m256d_u *)__P; +# | ^~~ +# In file included from include/eigen3/Eigen/Core:278: +# include/eigen3/Eigen/src/Core/AssignEvaluator.h: In member function ‘void Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map >; Derived = Eigen::Matrix]’: +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:850:41: note: at offset [0, 16] into object ‘tmp’ of size 24 +# 850 | typename plain_matrix_type::type tmp(src); +# | ^~~ +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12.0) + add_compile_options(-Wall -Werror -Wno-array-bounds) +else() + add_compile_options(-Wall -Werror) +endif() # fuse_constraints library add_library(${PROJECT_NAME} @@ -121,7 +162,6 @@ ament_export_dependencies( geometry_msgs pluginlib rclcpp - Ceres Eigen3 glog diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp index 864caef00..15f729ab7 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp @@ -38,14 +38,21 @@ #include #include +#include +#include #include +#include #include +#include #include +#include #include #include -#include #include +#include +#include +#include #include #include @@ -125,6 +132,7 @@ class MarginalConstraint : public fuse_core::Constraint */ const std::vector & x_bar() const {return x_bar_;} +#if !CERES_SUPPORTS_MANIFOLDS /** * @brief Read-only access to the variable local parameterizations */ @@ -132,6 +140,12 @@ class MarginalConstraint : public fuse_core::Constraint { return local_parameterizations_; } +#else + /** + * @brief Read-only access to the variable manifolds + */ + const std::vector & manifolds() const {return manifolds_;} +#endif /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -155,10 +169,12 @@ class MarginalConstraint : public fuse_core::Constraint protected: std::vector A_; //!< The A matrices of the marginal constraint fuse_core::VectorXd b_; //!< The b vector of the marginal constraint - +#if !CERES_SUPPORTS_MANIFOLDS //!< The local parameterizations std::vector local_parameterizations_; - +#else + std::vector manifolds_; //!< Manifolds +#endif std::vector x_bar_; //!< The linearization point of each involved variable private: @@ -166,21 +182,75 @@ class MarginalConstraint : public fuse_core::Constraint friend class boost::serialization::access; /** - * @brief The Boost Serialize method that serializes all of the data members in to/out of the - * archive + * @brief The Boost Serialize method that serializes all of the data members in to 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. + * @param[out] archive - The archive object into which class members will be serialized + * @param[in] version - The version of the archive being written. */ template - void serialize(Archive & archive, const unsigned int /* version */) + void save(Archive & archive, const unsigned int /* version */) const { - archive & boost::serialization::base_object(*this); - archive & A_; - archive & b_; - archive & local_parameterizations_; - archive & x_bar_; + archive << boost::serialization::base_object(*this); + archive << A_; + archive << b_; +#if !CERES_SUPPORTS_MANIFOLDS + archive << local_parameterizations_; +#else + archive << manifolds_; +#endif + archive << x_bar_; } + + /** + * @brief The Boost Serialize method that serializes all of the data members out of the archive + * + * @param[in] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read. + */ + template + void load(Archive & archive, const unsigned int version) + { + archive >> boost::serialization::base_object(*this); + archive >> A_; + archive >> b_; + if (version == 0) { + // Version 0 serialization files will contain a std::vector of LocalParameterization + // shared pointers. If the current version of Ceres Solver does not support Manifolds, + // then the serialized LocalParameterization pointers can be deserialized directly into + // the class member. But if the current version of Ceres Solver supports manifolds, then + // the serialized LocalParameterization pointers must be wrapped in a Manifold adapter first. +#if !CERES_SUPPORTS_MANIFOLDS + archive >> local_parameterizations_; +#else + auto local_parameterizations = std::vector(); + archive >> local_parameterizations; + std::transform( + std::make_move_iterator(local_parameterizations.begin()), + std::make_move_iterator(local_parameterizations.end()), + std::back_inserter(manifolds_), + [](fuse_core::LocalParameterization::SharedPtr local_parameterization) + {return fuse_core::ManifoldAdapter::make_shared(std::move(local_parameterization));}); +#endif + } else { // (version >= 1) + // Version 1 serialization files will contain a std::vector of Manifold shared pointers. If + // the current version of Ceres Solver does not support Manifolds, then there is no way to + // deserialize the requested data. But if the current version of Ceres Solver does support + // manifolds, then the serialized Manifold pointers can be deserialized directly into the + // class member. +#if !CERES_SUPPORTS_MANIFOLDS + throw std::runtime_error( + "Attempting to deserialize an archive saved in Version " + + std::to_string( + version) + " format. However, the current version of Ceres Solver (" + + CERES_VERSION_STRING + ") does not support manifolds. Ceres Solver version 2.1.0 " + "or later is required to load this file."); +#else + archive >> manifolds_; +#endif + } + archive >> x_bar_; + } + BOOST_SERIALIZATION_SPLIT_MEMBER() }; namespace detail @@ -202,14 +272,24 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & var return Eigen::Map(variable.data(), variable.size()); } +#if !CERES_SUPPORTS_MANIFOLDS /** * @brief Return the local parameterization of the provided variable */ -inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterization( +inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization( const fuse_core::Variable & variable) { return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization()); } +#else +/** + * @brief Return the manifold of the provided variable + */ +inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & variable) +{ + return fuse_core::Manifold::SharedPtr(variable.manifold()); +} +#endif } // namespace detail @@ -226,16 +306,26 @@ MarginalConstraint::MarginalConstraint( boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)), A_(first_A, last_A), b_(b), +#if !CERES_SUPPORTS_MANIFOLDS local_parameterizations_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getLocalParameterization), boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getLocalParameterization)), +#else + manifolds_( + boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)), +#endif x_bar_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue), boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue)) { assert(!A_.empty()); assert(A_.size() == x_bar_.size()); +#if !CERES_SUPPORTS_MANIFOLDS assert(A_.size() == local_parameterizations_.size()); +#else + assert(A_.size() == manifolds_.size()); +#endif assert(b_.rows() > 0); assert( std::all_of( @@ -255,5 +345,13 @@ MarginalConstraint::MarginalConstraint( } // namespace fuse_constraints BOOST_CLASS_EXPORT_KEY(fuse_constraints::MarginalConstraint); +// Since the contents of the serialized file will change depending on the CeresSolver version, +// also set the Boost Serialization version to allow code reading serialized file to know what +// data to expect. +#if !CERES_SUPPORTS_MANIFOLDS +BOOST_CLASS_VERSION(fuse_constraints::MarginalConstraint, 0); +#else +BOOST_CLASS_VERSION(fuse_constraints::MarginalConstraint, 1); +#endif #endif // FUSE_CONSTRAINTS__MARGINAL_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp index 072e97155..671ebf380 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp @@ -38,9 +38,10 @@ #include +#include #include #include - +#include namespace fuse_constraints { @@ -66,6 +67,7 @@ namespace fuse_constraints class MarginalCostFunction : public ceres::CostFunction { public: +#if !CERES_SUPPORTS_MANIFOLDS /** * @brief Construct a cost function instance * @@ -81,6 +83,21 @@ class MarginalCostFunction : public ceres::CostFunction const fuse_core::VectorXd & b, const std::vector & x_bar, const std::vector & local_parameterizations); +#else + /** + * @brief Construct a cost function instance + * + * @param[in] A The A matrix of the marginal cost (of the form A*(x - x_bar) + b) + * @param[in] b The b vector of the marginal cost (of the form A*(x - x_bar) + b) + * @param[in] x_bar The linearization point of the involved variables + * @param[in] manifolds The manifold associated with the variable + */ + MarginalCostFunction( + const std::vector & A, + const fuse_core::VectorXd & b, + const std::vector & x_bar, + const std::vector & manifolds); +#endif /** * @brief Destructor @@ -99,10 +116,12 @@ class MarginalCostFunction : public ceres::CostFunction private: const std::vector & A_; //!< The A matrices of the marginal cost const fuse_core::VectorXd & b_; //!< The b vector of the marginal cost - +#if !CERES_SUPPORTS_MANIFOLDS //!< Parameterizations const std::vector & local_parameterizations_; - +#else + const std::vector & manifolds_; //!< Manifolds +#endif const std::vector & x_bar_; //!< The linearization point of each variable }; diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp index 2656f127f..1cc666243 100644 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp +++ b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp @@ -47,9 +47,8 @@ #include #include #include -#include -#include #include +#include #include #include diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index 9af6a3f3e..0ee0e937e 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -68,7 +69,11 @@ void MarginalConstraint::print(std::ostream & stream) const ceres::CostFunction * MarginalConstraint::costFunction() const { +#if !CERES_SUPPORTS_MANIFOLDS return new MarginalCostFunction(A_, b_, x_bar_, local_parameterizations_); +#else + return new MarginalCostFunction(A_, b_, x_bar_, manifolds_); +#endif } } // namespace fuse_constraints diff --git a/fuse_constraints/src/marginal_cost_function.cpp b/fuse_constraints/src/marginal_cost_function.cpp index aa6578871..d148a5043 100644 --- a/fuse_constraints/src/marginal_cost_function.cpp +++ b/fuse_constraints/src/marginal_cost_function.cpp @@ -37,12 +37,14 @@ #include #include +#include #include #include +#include namespace fuse_constraints { - +#if !CERES_SUPPORTS_MANIFOLDS MarginalCostFunction::MarginalCostFunction( const std::vector & A, const fuse_core::VectorXd & b, @@ -58,6 +60,23 @@ MarginalCostFunction::MarginalCostFunction( mutable_parameter_block_sizes()->push_back(x_bar.size()); } } +#else +MarginalCostFunction::MarginalCostFunction( + const std::vector & A, + const fuse_core::VectorXd & b, + const std::vector & x_bar, + const std::vector & manifolds) +: A_(A), + b_(b), + manifolds_(manifolds), + x_bar_(x_bar) +{ + set_num_residuals(b_.rows()); + for (const auto & x_bar : x_bar_) { + mutable_parameter_block_sizes()->push_back(x_bar.size()); + } +} +#endif bool MarginalCostFunction::Evaluate( double const * const * parameters, @@ -69,8 +88,13 @@ bool MarginalCostFunction::Evaluate( residuals_map = b_; for (size_t i = 0; i < A_.size(); ++i) { fuse_core::VectorXd delta(A_[i].cols()); +#if !CERES_SUPPORTS_MANIFOLDS if (local_parameterizations_[i]) { local_parameterizations_[i]->Minus(x_bar_[i].data(), parameters[i], delta.data()); +#else + if (manifolds_[i]) { + manifolds_[i]->Minus(parameters[i], x_bar_[i].data(), delta.data()); +#endif } else { for (int j = 0; j < x_bar_[i].rows(); ++j) { delta[j] = parameters[i][j] - x_bar_[i][j]; @@ -83,11 +107,19 @@ bool MarginalCostFunction::Evaluate( if (jacobians) { for (size_t i = 0; i < A_.size(); ++i) { if (jacobians[i]) { +#if !CERES_SUPPORTS_MANIFOLDS if (local_parameterizations_[i]) { const auto & local_parameterization = local_parameterizations_[i]; fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), local_parameterization->GlobalSize()); local_parameterization->ComputeMinusJacobian(parameters[i], J_local.data()); +#else + if (manifolds_[i]) { + const auto & manifold = manifolds_[i]; + fuse_core::MatrixXd J_local(manifold->TangentSize(), + manifold->AmbientSize()); + manifold->MinusJacobian(parameters[i], J_local.data()); +#endif Eigen::Map( jacobians[i], num_residuals(), parameter_block_sizes()[i]) = A_[i] * J_local; diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 5eb93fb90..1614cfa5d 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -31,8 +31,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include #include #include @@ -44,17 +42,23 @@ #include #include +// The ROS 2 ament linter incorrectly recognizes Eigen includes as C instead of C++ #include #include +#include // NOLINT[build/include_order] +#include // NOLINT[build/include_order] + #include #include #include #include +#include +#include +#include #include namespace fuse_constraints { - UuidOrdering computeEliminationOrder( const std::vector & marginalized_variables, const fuse_core::Graph & graph) @@ -188,8 +192,8 @@ fuse_core::Transaction marginalizeVariables( [&elimination_order, &marginalized_variables](const fuse_core::UUID & variable_uuid) { return elimination_order.exists(variable_uuid) && - elimination_order.at(variable_uuid) < marginalized_variables.size(); - })); // NOLINT + elimination_order.at(variable_uuid) < marginalized_variables.size(); + })); // NOLINT fuse_core::Transaction transaction; @@ -327,6 +331,7 @@ LinearTerm linearize( for (size_t index = 0ul; index < variable_count; ++index) { const auto & variable_uuid = variable_uuids[index]; const auto & variable = graph.getVariable(variable_uuid); +#if !CERES_SUPPORTS_MANIFOLDS auto local_parameterization = variable.localParameterization(); auto & jacobian = result.A[index]; if (variable.holdConstant()) { @@ -343,6 +348,23 @@ LinearTerm linearize( if (local_parameterization) { delete local_parameterization; } +#else + auto manifold = variable.manifold(); + auto & jacobian = result.A[index]; + if (variable.holdConstant()) { + if (manifold) { + jacobian.resize(Eigen::NoChange, manifold->TangentSize()); + } + jacobian.setZero(); + } else if (manifold) { + fuse_core::MatrixXd J(manifold->AmbientSize(), manifold->TangentSize()); + manifold->PlusJacobian(variable_values[index], J.data()); + jacobian *= J; + } + if (manifold) { + delete manifold; + } +#endif } // Correct A and b for the effects of the loss function diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp index 8be6b3f49..5a80953fd 100755 --- a/fuse_constraints/src/variable_constraints.cpp +++ b/fuse_constraints/src/variable_constraints.cpp @@ -52,9 +52,9 @@ bool VariableConstraints::empty() const size_t VariableConstraints::size() const { auto sum_edges = [](const size_t input, const ConstraintCollection & edges) - { - return input + edges.size(); - }; + { + return input + edges.size(); + }; return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges); } diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 119c78ef6..ec69a351b 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -1,6 +1,7 @@ # CORE GTESTS ====================================================================================== set(TEST_TARGETS test_absolute_constraint + test_absolute_orientation_2d_stamped_constraint test_absolute_orientation_3d_stamped_constraint test_absolute_orientation_3d_stamped_euler_constraint test_absolute_pose_2d_stamped_constraint @@ -21,6 +22,6 @@ set(TEST_TARGETS ) foreach(test_name ${TEST_TARGETS}) - ament_add_gtest("${test_name}" "${test_name}.cpp") + ament_add_gtest("${test_name}" "${test_name}.cpp" WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) target_link_libraries("${test_name}" ${PROJECT_NAME}) endforeach() diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 791c62fe4..02cae90e3 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -50,12 +50,10 @@ * * @param[in] cost_function The expected cost function * @param[in] actual_cost_function The actual cost function - * @param[in] tolerance The tolerance to use when comparing the cost functions are equal. Defaults - * to 1e-18 */ static void ExpectCostFunctionsAreEqual( const ceres::CostFunction & cost_function, - const ceres::CostFunction & actual_cost_function, double tolerance = 1e-18) + const ceres::CostFunction & actual_cost_function) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); @@ -113,7 +111,7 @@ static void ExpectCostFunctionsAreEqual( parameter_blocks.get(), actual_residuals.get(), nullptr)); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_NEAR(residuals[i], actual_residuals[i], tolerance) << "residual id: " << i; + EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; } EXPECT_TRUE( @@ -125,11 +123,11 @@ static void ExpectCostFunctionsAreEqual( parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_NEAR(residuals[i], actual_residuals[i], tolerance) << "residual : " << i; + EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; } for (size_t i = 0; i < num_residuals * num_parameters; ++i) { - EXPECT_NEAR(jacobians[i], actual_jacobians[i], tolerance) + EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } diff --git a/fuse_constraints/test/legacy_marginal_version0.txt b/fuse_constraints/test/legacy_marginal_version0.txt new file mode 100644 index 000000000..924f51709 --- /dev/null +++ b/fuse_constraints/test/legacy_marginal_version0.txt @@ -0,0 +1,3 @@ +22 serialization::archive 18 1 0 +0 0 0 4 test 893e881d-2f6d-416d-bcad-2aec3a1d69da 0 0 1 0 f585f09e-8f7b-5d41-890f-0ae773b82097 0 1 -1 0 0 1 0 0 0 1 3 5.00000000000000000e+00 6.00000000000000000e+00 7.00000000000000000e+00 0 0 1 1 8.00000000000000000e+00 0 0 1 1 0 1 9 50 fuse_variables::Orientation3DLocalParameterization 1 0 +1 0 0 0 0 1 0 4 1 8.42614976999999987e-01 2.00000000000000011e-01 2.99999999999999989e-01 4.00000000000000022e-01 diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index 7b113552a..9fe209943 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -229,7 +230,11 @@ TEST(AbsoluteConstraint, Optimization) problem.AddParameterBlock( variable->data(), variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS variable->localParameterization()); +#else + variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); problem.AddResidualBlock( @@ -297,7 +302,11 @@ TEST(AbsoluteConstraint, Optimization) problem.AddParameterBlock( var->data(), var->size(), +#if !CERES_SUPPORTS_MANIFOLDS var->localParameterization()); +#else + var->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(var->data()); problem.AddResidualBlock( @@ -373,7 +382,11 @@ TEST(AbsoluteConstraint, PartialOptimization) problem.AddParameterBlock( var->data(), var->size(), +#if !CERES_SUPPORTS_MANIFOLDS var->localParameterization()); +#else + var->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(var->data()); problem.AddResidualBlock( @@ -416,7 +429,11 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) problem.AddParameterBlock( variable->data(), variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS variable->localParameterization()); +#else + variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); problem.AddResidualBlock( diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp new file mode 100644 index 000000000..dbe4b0ab0 --- /dev/null +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -0,0 +1,398 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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. + */ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using fuse_constraints::AbsoluteOrientation2DStampedConstraint; +using fuse_variables::Orientation2DStamped; + + +TEST(AbsoluteOrientation2DStampedConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + EXPECT_NO_THROW( + AbsoluteOrientation2DStampedConstraint constraint( + "test", orientation_variable, + mean, cov)); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix1d expected_sqrt_info; + expected_sqrt_info << 1.0; + fuse_core::Matrix1d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->yaw() = 1.0; + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 1.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(1.0, orientation_variable->yaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) +{ + // Optimize a single orientation at zero and single constraint, verify the expected value and + // covariance are generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), + fuse_core::uuid::generate("spra")); + orientation_variable->yaw() = 0.0; + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 0.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(0.0, orientation_variable->yaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +// Temporarily disable this unit test. This should be fixed by PR #335. +// TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) +// { +// // Optimize a single orientation at +PI and single constraint, verify the expected value and +// // covariance are generated. +// +// // Create the variables +// auto orientation_variable = Orientation2DStamped::make_shared( +// rclcpp::Time(1, 0), +// fuse_core::uuid::generate("spra")); +// orientation_variable->yaw() = M_PI; +// +// // Create an absolute orientation constraint +// fuse_core::Vector1d mean; +// mean << M_PI; +// +// fuse_core::Matrix1d cov; +// cov << 1.0; +// auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( +// "test", +// *orientation_variable, +// mean, +// cov); +// +// // Build the problem +// ceres::Problem::Options problem_options; +// problem_options.loss_function_ownership = fuse_core::Loss::Ownership; +// ceres::Problem problem(problem_options); +// problem.AddParameterBlock( +// orientation_variable->data(), +// orientation_variable->size(), +// #if !CERES_SUPPORTS_MANIFOLDS +// orientation_variable->localParameterization()); +// #else +// orientation_variable->manifold()); +// #endif +// +// std::vector parameter_blocks; +// parameter_blocks.push_back(orientation_variable->data()); +// problem.AddResidualBlock( +// constraint->costFunction(), +// constraint->lossFunction(), +// parameter_blocks); +// +// // Run the solver +// ceres::Solver::Options options; +// ceres::Solver::Summary summary; +// ceres::Solve(options, &problem, &summary); +// EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); +// +// // Check +// // We expect +PI to roll over to -PI because our range is [-PI, PI) +// EXPECT_NEAR(-M_PI, orientation_variable->yaw(), 1.0e-3); +// +// // Compute the covariance +// std::vector> covariance_blocks; +// covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); +// ceres::Covariance::Options cov_options; +// ceres::Covariance covariance(cov_options); +// covariance.Compute(covariance_blocks, &problem); +// fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), +// orientation_variable->localSize()); +// covariance.GetCovarianceBlockInTangentSpace( +// orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); +// +// // Define the expected covariance +// fuse_core::Matrix1d expected_covariance; +// expected_covariance << 1.0; +// EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +// } + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) +{ + // Optimize a single orientation at -PI and single constraint, verify the expected value and + // covariance are generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), + fuse_core::uuid::generate("spra")); + orientation_variable->yaw() = -M_PI; + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << -M_PI; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(-M_PI, orientation_variable->yaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Serialization) +{ + // Construct a constraint + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint expected("test", orientation_variable, mean, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + AbsoluteOrientation2DStampedConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp index c3b793d5c..0a10bbbb5 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -139,7 +140,11 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 36f149456..9d775cb5e 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -131,7 +132,11 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); @@ -203,7 +208,11 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); diff --git a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp index 927c24517..ef88fe0dd 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -122,11 +123,19 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif problem.AddParameterBlock( position_variable->data(), position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); @@ -229,12 +238,19 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) problem.AddParameterBlock( position_variable->data(), position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); - +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index 9aa40bcf1..e13c14186 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -167,11 +168,19 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) problem.AddParameterBlock( position_variable->data(), position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); diff --git a/fuse_constraints/test/test_marginal_constraint.cpp b/fuse_constraints/test/test_marginal_constraint.cpp index 7e8e13e01..cf49f9ef0 100644 --- a/fuse_constraints/test/test_marginal_constraint.cpp +++ b/fuse_constraints/test/test_marginal_constraint.cpp @@ -33,12 +33,15 @@ */ #include +#include #include #include #include +#include #include #include +#include #include #include #include @@ -202,7 +205,10 @@ TEST(MarginalConstraint, LocalParameterization) fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 4); std::vector actual_jacobians = {actual_jacobian1.data()}; - cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); + cost_function->Evaluate( + variable_values.data(), + actual_residuals.data(), + actual_jacobians.data()); // Define the expected residuals and jacobians fuse_core::Vector1d expected_residuals; @@ -268,6 +274,7 @@ TEST(MarginalConstraint, Serialization) EXPECT_EQ(expected.b(), actual.b()); EXPECT_EQ(expected.x_bar(), actual.x_bar()); // The shared ptrs will not be the same instances, but they should point to the same types +#if !CERES_SUPPORTS_MANIFOLDS using ExpectedLocalParam = fuse_variables::Orientation3DLocalParameterization; ASSERT_EQ(expected.localParameterizations().size(), actual.localParameterizations().size()); for (auto i = 0u; i < actual.localParameterizations().size(); ++i) { @@ -275,4 +282,73 @@ TEST(MarginalConstraint, Serialization) actual.localParameterizations()[i]); EXPECT_TRUE(static_cast(actual_derived)); } +#else + using ExpectedManifold = fuse_variables::Orientation3DManifold; + ASSERT_EQ(expected.manifolds().size(), actual.manifolds().size()); + for (auto i = 0u; i < actual.manifolds().size(); ++i) { + auto actual_derived = std::dynamic_pointer_cast(actual.manifolds()[i]); + EXPECT_TRUE(static_cast(actual_derived)); + } +#endif +} + +#if CERES_SUPPORTS_MANIFOLDS +TEST(MarginalConstraint, LegacyDeserialization) +{ + // Test deserializing a marginal constraint generated from an older version + // of Ceres Solver + + std::vector variables; + fuse_variables::Orientation3DStamped x1(rclcpp::Time(1, 0)); + x1.w() = 0.842614977; + x1.x() = 0.2; + x1.y() = 0.3; + x1.z() = 0.4; + variables.push_back(x1); + + std::vector A; + fuse_core::MatrixXd A1(1, 3); + A1 << 5.0, 6.0, 7.0; + A.push_back(A1); + + fuse_core::Vector1d b; + b << 8.0; + + auto expected = fuse_constraints::MarginalConstraint( + "test", + variables.begin(), + variables.end(), + A.begin(), + A.end(), + b); + + // The legacy serialization file was generated using the following code: + // { + // std::ofstream output_file("legacy_marginal_version0.txt"); + // fuse_core::TextOutputArchive archive(output_file); + // expected.serialize(archive); + // } + + // Deserialize a new constraint from that same stream + fuse_constraints::MarginalConstraint actual; + { + std::ifstream input_file("legacy_marginal_version0.txt"); + fuse_core::TextInputArchive archive(input_file); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_EQ(expected.A(), actual.A()); + EXPECT_EQ(expected.b(), actual.b()); + EXPECT_EQ(expected.x_bar(), actual.x_bar()); + // When deserializing the legacy file, the old local parameterizations + // should get wrapped in a ManifoldAdpater + using ExpectedManifold = fuse_core::ManifoldAdapter; + ASSERT_EQ(expected.manifolds().size(), actual.manifolds().size()); + for (auto i = 0u; i < actual.manifolds().size(); ++i) { + auto actual_derived = std::dynamic_pointer_cast(actual.manifolds()[i]); + EXPECT_TRUE(static_cast(actual_derived)); + } } +#endif diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 821f2dfe5..e221c4c58 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -266,11 +267,19 @@ TEST(RelativeConstraint, Optimization) problem.AddParameterBlock( x1->data(), x1->size(), +#if !CERES_SUPPORTS_MANIFOLDS x1->localParameterization()); +#else + x1->manifold()); +#endif problem.AddParameterBlock( x2->data(), x2->size(), +#if !CERES_SUPPORTS_MANIFOLDS x2->localParameterization()); +#else + x2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); problem.AddResidualBlock( @@ -375,11 +384,19 @@ TEST(RelativeConstraint, Optimization) problem.AddParameterBlock( x1->data(), x1->size(), +#if !CERES_SUPPORTS_MANIFOLDS x1->localParameterization()); +#else + x1->manifold()); +#endif problem.AddParameterBlock( x2->data(), x2->size(), +#if !CERES_SUPPORTS_MANIFOLDS x2->localParameterization()); +#else + x2->manifold()); +#endif std::vector c1_parameter_blocks; c1_parameter_blocks.push_back(x1->data()); problem.AddResidualBlock( @@ -476,11 +493,19 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) problem.AddParameterBlock( x1->data(), x1->size(), +#if !CERES_SUPPORTS_MANIFOLDS x1->localParameterization()); +#else + x1->manifold()); +#endif problem.AddParameterBlock( x2->data(), x2->size(), +#if !CERES_SUPPORTS_MANIFOLDS x2->localParameterization()); +#else + x2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); problem.AddResidualBlock( diff --git a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp index 4850a0367..014ad5387 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -153,19 +154,35 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) problem.AddParameterBlock( orientation1->data(), orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif problem.AddParameterBlock( position1->data(), position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS position1->localParameterization()); +#else + position1->manifold()); +#endif problem.AddParameterBlock( orientation2->data(), orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif problem.AddParameterBlock( position2->data(), position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS position2->localParameterization()); +#else + position2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); @@ -338,19 +355,35 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) problem.AddParameterBlock( orientation1->data(), orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif problem.AddParameterBlock( position1->data(), position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS position1->localParameterization()); +#else + position1->manifold()); +#endif problem.AddParameterBlock( orientation2->data(), orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif problem.AddParameterBlock( position2->data(), position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS position2->localParameterization()); +#else + position2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp index c815fcc34..7e16d7e56 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -193,19 +194,35 @@ TEST(RelativePose3DStampedConstraint, Optimization) problem.AddParameterBlock( orientation1->data(), orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif problem.AddParameterBlock( position1->data(), position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS position1->localParameterization()); +#else + position1->manifold()); +#endif problem.AddParameterBlock( orientation2->data(), orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif problem.AddParameterBlock( position2->data(), position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS position2->localParameterization()); +#else + position2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index c9b555d3c..737542c0a 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -38,6 +38,7 @@ add_library(${PROJECT_NAME} src/graph.cpp src/graph_deserializer.cpp src/loss.cpp + src/manifold_adapter.cpp src/parameter.cpp src/serialization.cpp src/timestamp_manager.cpp diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index ae2929598..06fbd1209 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -109,7 +109,7 @@ template class CallbackWrapper : public CallbackWrapperBase { public: - using CallbackFunction = std::function; + using CallbackFunction = std::function; /** * @brief Constructor diff --git a/fuse_core/include/fuse_core/ceres_macros.hpp b/fuse_core/include/fuse_core/ceres_macros.hpp index aff1274f1..d5c648204 100644 --- a/fuse_core/include/fuse_core/ceres_macros.hpp +++ b/fuse_core/include/fuse_core/ceres_macros.hpp @@ -46,4 +46,6 @@ CERES_VERSION_REVISION >= z)))) /* *INDENT-ON* */ +#define CERES_SUPPORTS_MANIFOLDS CERES_VERSION_AT_LEAST(2, 1, 0) + #endif // FUSE_CORE__CERES_MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp index 597a5ba8e..6b7853f3c 100644 --- a/fuse_core/include/fuse_core/ceres_options.hpp +++ b/fuse_core/include/fuse_core/ceres_options.hpp @@ -54,10 +54,10 @@ * For a given Ceres Solver Option , the function ToString calls ceres::ToString */ #define CERES_OPTION_TO_STRING_DEFINITION(Option) \ - static inline const char * ToString(ceres::Option value) \ - { \ - return ceres::Option ## ToString(value); \ - } + static inline const char * ToString(ceres::Option value) \ + { \ + return ceres::Option ## ToString(value); \ + } /** * Defines FromString overloaded function to Ceres Options. @@ -65,10 +65,10 @@ * For a given Ceres Solver Option , the function FromString calls ceres::StringTo */ #define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ - static inline bool FromString(std::string string_value, ceres::Option * value) \ - { \ - return ceres::StringTo ## Option(string_value, value); \ - } + static inline bool FromString(std::string string_value, ceres::Option * value) \ + { \ + return ceres::StringTo ## Option(string_value, value); \ + } /** * Defines ToString and FromString overloaded functions for Ceres Options. @@ -76,8 +76,8 @@ * See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION. */ #define CERES_OPTION_STRING_DEFINITIONS(Option) \ - CERES_OPTION_TO_STRING_DEFINITION(Option) \ - CERES_OPTION_FROM_STRING_DEFINITION(Option) + CERES_OPTION_TO_STRING_DEFINITION(Option) \ + CERES_OPTION_FROM_STRING_DEFINITION(Option) #if !CERES_VERSION_AT_LEAST(2, 0, 0) /** @@ -99,8 +99,8 @@ static void UpperCase(std::string * input) inline const char * LoggingTypeToString(LoggingType type) { switch (type) { - CASESTR(SILENT); - CASESTR(PER_MINIMIZER_ITERATION); + CASESTR(SILENT); + CASESTR(PER_MINIMIZER_ITERATION); default: return "UNKNOWN"; } @@ -117,8 +117,8 @@ inline bool StringToLoggingType(std::string value, LoggingType * type) inline const char * DumpFormatTypeToString(DumpFormatType type) { switch (type) { - CASESTR(CONSOLE); - CASESTR(TEXTFILE); + CASESTR(CONSOLE); + CASESTR(TEXTFILE); default: return "UNKNOWN"; } diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 1abd6d16d..553e3dfb6 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -67,10 +67,10 @@ * @endcode */ #define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ - fuse_core::Constraint::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Constraint::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -86,22 +86,22 @@ * @endcode */ #define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -119,17 +119,17 @@ * @endcode */ #define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -146,10 +146,10 @@ * @endcode */ #define FUSE_CONSTRAINT_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -166,10 +166,10 @@ * @endcode */ #define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp index 5fe3d760c..7205470ff 100644 --- a/fuse_core/include/fuse_core/eigen_gtest.hpp +++ b/fuse_core/include/fuse_core/eigen_gtest.hpp @@ -108,23 +108,23 @@ AssertionResult AssertMatrixNearHelper( // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ. // Don't use this in your code. #define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixEqualHelper( \ - #v1, \ - #v2, \ - v1, \ - v2), on_failure) + GTEST_ASSERT_( \ + ::testing::AssertMatrixEqualHelper( \ + #v1, \ + #v2, \ + v1, \ + v2), on_failure) // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR. // Don't use this in your code. #define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixNearHelper( \ - #v1, \ - #v2, \ - v1, \ - v2, \ - tol), on_failure) + GTEST_ASSERT_( \ + ::testing::AssertMatrixNearHelper( \ + #v1, \ + #v2, \ + v1, \ + v2, \ + tol), on_failure) // Define gtest macros for use with Eigen @@ -137,7 +137,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix */ #define EXPECT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) + GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for exact equality of two Eigen matrix-like objects. @@ -148,7 +148,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix */ #define ASSERT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) + GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) /** * @brief Non-fatal check for approximate equality of two Eigen matrix-like objects. @@ -160,7 +160,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ #define EXPECT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) + GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for approximate equality of two Eigen matrix-like objects. @@ -172,7 +172,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ #define ASSERT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) + GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) } // namespace testing diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp index 9cc026637..a896a4c75 100644 --- a/fuse_core/include/fuse_core/fuse_macros.hpp +++ b/fuse_core/include/fuse_core/fuse_macros.hpp @@ -86,11 +86,11 @@ * Use in the public section of the class. */ #define FUSE_SMART_PTR_DEFINITIONS(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** * Defines smart pointer aliases and static functions for a class that contains fixed-sized @@ -104,15 +104,15 @@ */ #if __cpp_aligned_new #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) #else #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -124,55 +124,55 @@ * Use in the public section of the class. */ #define FUSE_SMART_PTR_ALIASES_ONLY(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) #define __FUSE_SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ - using ConstSharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; #define __FUSE_MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } #define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>( \ + Eigen::aligned_allocator<__VA_ARGS__>(), \ + std::forward(args) ...); \ + } #define __FUSE_WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ - using ConstWeakPtr = std::weak_ptr; + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; #define __FUSE_UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; + using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ + } #else #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } #endif #endif // FUSE_CORE__FUSE_MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index a9f366c84..4b9c96c0f 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -71,22 +71,22 @@ * @endcode */ #define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -104,17 +104,17 @@ * @endcode */ #define FUSE_GRAPH_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, and type() method @@ -130,9 +130,9 @@ * @endcode */ #define FUSE_GRAPH_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/local_parameterization.hpp b/fuse_core/include/fuse_core/local_parameterization.hpp index 03f09e636..c6cebcc03 100644 --- a/fuse_core/include/fuse_core/local_parameterization.hpp +++ b/fuse_core/include/fuse_core/local_parameterization.hpp @@ -34,12 +34,20 @@ #ifndef FUSE_CORE__LOCAL_PARAMETERIZATION_HPP_ #define FUSE_CORE__LOCAL_PARAMETERIZATION_HPP_ -#include - #include +#include #include #include +#if !CERES_SUPPORTS_MANIFOLDS +// Local parameterizations is removed in favour of Manifold in +// version 2.2.0, see +// https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc +#include +#else +#include +#endif + namespace fuse_core { @@ -54,33 +62,80 @@ namespace fuse_core * See the Ceres documentation for more details. http://ceres- * solver.org/nnls_modeling.html#localparameterization */ -class LocalParameterization : public ceres::LocalParameterization +class LocalParameterization +// extend ceres LocalParameterization if our version of Ceres does not support Manifolds +#if !CERES_SUPPORTS_MANIFOLDS + : public ceres::LocalParameterization +#endif { public: FUSE_SMART_PTR_ALIASES_ONLY(LocalParameterization) + /** + * @brief Destroy the Local Parameterization object + */ + virtual ~LocalParameterization() = default; + + /** + * @brief Size of x + * + * @return int Size of x. + */ + virtual int GlobalSize() const = 0; + + /** + * @brief Size of delta + * + * @return int Size of delta + */ + virtual int LocalSize() const = 0; + + /** + * @brief Generalization of the addition operation, + * + * x_plus_delta = Plus(x, delta) + * + * with the condition that Plus(x, 0) = x. + * @param[in] x variable of size \p GlobalSize() + * @param[in] delta variable of size \p LocalSize() + * @param[out] x_plus_delta of size \p GlobalSize() + */ + virtual bool Plus( + const double * x, + const double * delta, + double * x_plus_delta) const = 0; + + /** + * @brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + * + * @param[in] x variable of size \p GlobalSize() + * @param[out] jacobian a row-major GlobalSize() x LocalSize() matrix. + * @return + */ + virtual bool ComputeJacobian(const double * x, double * jacobian) const = 0; + /** * @brief Generalization of the subtraction operation * - * Minus(x1, x2) -> delta + * Minus(x, y) -> y_minus_x * * with the conditions that: * - Minus(x, x) -> 0 - * - if Plus(x1, delta) -> x2, then Minus(x1, x2) -> delta + * - if Plus(x, delta) -> y, then Minus(x, y) -> delta * - * @param[in] x1 The value of the first variable, of size \p GlobalSize() - * @param[in] x2 The value of the second variable, of size \p GlobalSize() - * @param[out] delta The difference between the second variable and the first, of size \p - * LocalSize() + * @param[in] x The value of the first variable, of size \p GlobalSize() + * @param[in] y The value of the second variable, of size \p GlobalSize() + * @param[out] y_minus_x The difference between the second variable and the first, of size + * \p LocalSize() * @return True if successful, false otherwise */ virtual bool Minus( - const double * x1, - const double * x2, - double * delta) const = 0; + const double * x, + const double * y, + double * y_minus_x) const = 0; /** - * @brief The jacobian of Minus(x1, x2) w.r.t x2 at x1 == x2 == x + * @brief The jacobian of Minus(x, y) w.r.t y at x == y * * @param[in] x The value used to evaluate the Jacobian, of size \p GlobalSize() * @param[out] jacobian The first-order derivative in row-major order, of size \p LocalSize() x \p @@ -91,6 +146,49 @@ class LocalParameterization : public ceres::LocalParameterization const double * x, double * jacobian) const = 0; +#if CERES_SUPPORTS_MANIFOLDS + // If the fuse::LocalParameterization class does not inherit from the + // ceres::LocalParameterization class then we need to provide the default implementation of the + // MultiplyByJacobian() method + + /** + * @brief Computes local_matrix = global_matrix * jacobian + * + * This is only used by GradientProblem. For most normal uses, it is + * okay to use the default implementation. + * + * jacobian(x) is the matrix returned by ComputeJacobian at x. + * + * @param[in] x + * @param[in] num_rows + * @param[in] global_matrix is a num_rows x GlobalSize row major matrix. + * @param[out] local_matrix is a num_rows x LocalSize row major matrix. + */ + virtual bool MultiplyByJacobian( + const double * x, + const int num_rows, + const double * global_matrix, + double * local_matrix) const + { + using Matrix = Eigen::Matrix; + using MatrixRef = Eigen::Map; + using ConstMatrixRef = Eigen::Map; + + if (LocalSize() == 0) { + return true; + } + + Matrix jacobian(GlobalSize(), LocalSize()); + if (!ComputeJacobian(x, jacobian.data())) { + return false; + } + + MatrixRef(local_matrix, num_rows, LocalSize()) = + ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + return true; + } +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -103,9 +201,7 @@ class LocalParameterization : public ceres::LocalParameterization * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive & /* archive */, const unsigned int /* version */) - { - } + void serialize(Archive & /* archive */, const unsigned int /* version */) {} }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index 8465e574f..71812a5bf 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -59,10 +59,10 @@ * @endcode */ #define FUSE_LOSS_CLONE_DEFINITION(...) \ - fuse_core::Loss::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Loss::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -78,22 +78,22 @@ * @endcode */ #define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -111,17 +111,17 @@ * @endcode */ #define FUSE_LOSS_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -138,10 +138,10 @@ * @endcode */ #define FUSE_LOSS_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/macros.hpp b/fuse_core/include/fuse_core/macros.hpp index 3a46ed800..c78eb0966 100644 --- a/fuse_core/include/fuse_core/macros.hpp +++ b/fuse_core/include/fuse_core/macros.hpp @@ -90,11 +90,11 @@ * Use in the public section of the class. */ #define SMART_PTR_DEFINITIONS(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** * Defines smart pointer aliases and static functions for a class that contains fixed-sized @@ -108,15 +108,15 @@ */ #if __cpp_aligned_new #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - SMART_PTR_DEFINITIONS(__VA_ARGS__) + SMART_PTR_DEFINITIONS(__VA_ARGS__) #else #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -128,55 +128,55 @@ * Use in the public section of the class. */ #define SMART_PTR_ALIASES_ONLY(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) #define __SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ - using ConstSharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; #define __MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } #define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>( \ + Eigen::aligned_allocator<__VA_ARGS__>(), \ + std::forward(args) ...); \ + } #define __WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ - using ConstWeakPtr = std::weak_ptr; + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; #define __UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; + using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ + } #else #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } #endif #endif // FUSE_CORE__MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/manifold.h b/fuse_core/include/fuse_core/manifold.h new file mode 100644 index 000000000..be76e2b9c --- /dev/null +++ b/fuse_core/include/fuse_core/manifold.h @@ -0,0 +1,41 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Clearpath 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_CORE__MANIFOLD_H_ +#define FUSE_CORE__MANIFOLD_H_ + +#warning This header is obsolete, please include fuse_core/manifold.hpp instead + +#include + +#endif // FUSE_CORE__MANIFOLD_H_ diff --git a/fuse_core/include/fuse_core/manifold.hpp b/fuse_core/include/fuse_core/manifold.hpp new file mode 100644 index 000000000..d62b0da96 --- /dev/null +++ b/fuse_core/include/fuse_core/manifold.hpp @@ -0,0 +1,178 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Clearpath 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_CORE__MANIFOLD_HPP_ +#define FUSE_CORE__MANIFOLD_HPP_ + +#include + +#if CERES_SUPPORTS_MANIFOLDS + +#include +// Local parameterizations were marked as deprecated in favour of Manifold in +// version 2.1.0, see +// https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc +// and Local parameterizations were removed in 2.2.0, see +// https://github.com/ceres-solver/ceres-solver/commit/68c53bb39552cd4abfd6381df08638285f7386b3 +#include // NOLINT[build/include_order] + +#include +#include + +namespace fuse_core +{ +/** + * @brief The Manifold interface definition. + * + * In Ceres Solver version 2.1, the ceres::Manifold class was introduced, and in version 2.2 the + * ceres::LocalParameterization was deprecated. In a similar way that the fuse::LocalParameterization + * class wraps the ceres::LocalParameterization class, this class wraps the ceres::Manifold class + * for use within the Fuse codebase. + * + * Conceptually, the LocalParameterization class and Manifold class represent the same concept -- + * switching between a nonlinear manifold and a linear approximation of the manifold tangent to + * the manifold at a particular linearization point. + * + * The ceres::Manifold class renames a few members and adds a little more functionality. + * - ceres::LocalParameterization::GlobalSize becomes ceres::Manifold::AmbientSize + * - ceres::LocalParameterization::LocalSize becomes ceres::Manifold::TangentSize + * - ceres::LocalParameterization::ComputeJacobian() becomes ceres::Manifold::PlusJacobian() + * - ceres::Manifold adds Minus() and MinusJacobian() methods + * + * Note that the fuse::LocalParameterization has always had Minus() and ComputeMinusJacobian() + * methods. However, the ceres::Manifold function signatures are slightly different from the + * fuse::LocalParameterization function signatures. + * + * See the Ceres documentation for more details. + * http://ceres-solver.org/nnls_modeling.html#manifold + */ +class Manifold : public ceres::Manifold +{ +public: + FUSE_SMART_PTR_ALIASES_ONLY(Manifold) + + /** + * @brief Destroy the Manifold object + */ + virtual ~Manifold() = default; + + /** + * @brief Dimension of the ambient space in which the manifold is embedded. + * + * @return int Dimension of the ambient space in which the manifold is embedded. + */ + virtual int AmbientSize() const = 0; + + /** + * @brief Dimension of the manifold/tangent space. + * + * @return int Dimension of the manifold/tangent space. + */ + virtual int TangentSize() const = 0; + + /** + * @brief x_plus_delta = Plus(x, delta), + * + * A generalization of vector addition in Euclidean space, Plus computes the + * result of moving along delta in the tangent space at x, and then projecting + * back onto the manifold that x belongs to. + * + * @param[in] x is a \p AmbientSize() vector. + * @param[in] delta delta is a \p TangentSize() vector. + * @param[out] x_plus_delta is a \p AmbientSize() vector. + * @return Return value indicates if the operation was successful or not. + */ + virtual bool Plus(const double * x, const double * delta, double * x_plus_delta) const = 0; + + /** + * @brief Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, + * i.e. + * + * (D_2 Plus)(x, 0) + * + * @param[in] x is a \p AmbientSize() vector + * @param[out] jacobian is a row-major \p AmbientSize() x \p TangentSize() + * matrix. + * @return + */ + virtual bool PlusJacobian(const double * x, double * jacobian) const = 0; + + /** + * @brief Generalization of the subtraction operation + * + * Minus(y, x) -> y_minus_x + * + * with the conditions that: + * - Minus(x, x) -> 0 + * - if Plus(x, delta) -> y, then Minus(y, x) -> delta + * + * @param[in] y is a \p AmbientSize() vector. + * @param[in] x is a \p AmbientSize() vector. + * @param[out] y_minus_x is a \p TangentSize() vector. + * @return Return value indicates if the operation was successful or not. + */ + virtual bool Minus(const double * y, const double * x, double * y_minus_x) const = 0; + + /** + * @brief Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e + * + * (D_1 Minus) (y, y) + * + * @param[in] x is a \p AmbientSize() vector. + * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. + * @return Return value indicates whether the operation was successful or not. + */ + virtual bool MinusJacobian(const double * x, double * jacobian) const = 0; + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & /* archive */, const unsigned int /* version */) {} +}; + +} // namespace fuse_core + +#endif + +#endif // FUSE_CORE__MANIFOLD_HPP_ diff --git a/fuse_core/include/fuse_core/manifold_adapter.h b/fuse_core/include/fuse_core/manifold_adapter.h new file mode 100644 index 000000000..e6055bca7 --- /dev/null +++ b/fuse_core/include/fuse_core/manifold_adapter.h @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, 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_CORE__MANIFOLD_ADAPTER_H_ +#define FUSE_CORE__MANIFOLD_ADAPTER_H_ + +#warning This header is obsolete, please include fuse_core/manifold_adapter.hpp instead + +#include + +#endif // FUSE_CORE__MANIFOLD_ADAPTER_H_ diff --git a/fuse_core/include/fuse_core/manifold_adapter.hpp b/fuse_core/include/fuse_core/manifold_adapter.hpp new file mode 100644 index 000000000..730c02bc7 --- /dev/null +++ b/fuse_core/include/fuse_core/manifold_adapter.hpp @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Clearpath 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_CORE__MANIFOLD_ADAPTER_HPP_ +#define FUSE_CORE__MANIFOLD_ADAPTER_HPP_ + +#include + +#if CERES_SUPPORTS_MANIFOLDS +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace fuse_core +{ +class ManifoldAdapter : public fuse_core::Manifold +{ +public: + FUSE_SMART_PTR_DEFINITIONS(ManifoldAdapter) + + /** + * @brief Constructor to adapt a fuse::LocalParameterization into a fuse::Manifold + * + * The ManifoldAdapter will take ownership of the provided LocalParameterization and + * will delete it when the ManifoldAdapter goes out of scope. + * + * @param[in] local_parameterization Raw pointer to a derviced fuse::LocalParameterization object + */ + explicit ManifoldAdapter(fuse_core::LocalParameterization * local_parameterization) + { + local_parameterization_.reset(local_parameterization); + } + + /** + * @brief Constructor to adapt a fuse::LocalParameterization into a fuse::Manifold + * + * The ManifoldAdapter will share ownership of the provided LocalParameterization object. + * + * @param[in] local_parameterization Shared pointer to a derived fuse::LocalParameterization object + */ + explicit ManifoldAdapter(fuse_core::LocalParameterization::SharedPtr local_parameterization) + { + local_parameterization_ = std::move(local_parameterization); + } + + /** + * @brief Dimension of the ambient space in which the manifold is embedded. + * + * This is equivalent to the GlobalSize property of the LocalParameterization. + * + * @return int Dimension of the ambient space in which the manifold is embedded. + */ + int AmbientSize() const override {return local_parameterization_->GlobalSize();} + + /** + * @brief Dimension of the manifold/tangent space. + * + * This is equivalent to the LocalSize property of the LocalParameterization. + * + * @return int Dimension of the manifold/tangent space. + */ + int TangentSize() const override {return local_parameterization_->LocalSize();} + + /** + * @brief x_plus_delta = Plus(x, delta), + * + * A generalization of vector addition in Euclidean space, Plus computes the + * result of moving along delta in the tangent space at x, and then projecting + * back onto the manifold that x belongs to. + * + * @param[in] x is a \p AmbientSize() vector. + * @param[in] delta delta is a \p TangentSize() vector. + * @param[out] x_plus_delta is a \p AmbientSize() vector. + * @return Return value indicates if the operation was successful or not. + */ + bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + { + return local_parameterization_->Plus(x, delta, x_plus_delta); + } + + /** + * @brief Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, + * i.e. + * + * (D_2 Plus)(x, 0) + * + * @param[in] x is a \p AmbientSize() vector + * @param[out] jacobian is a row-major \p AmbientSize() x \p TangentSize() + * matrix. + * @return + */ + bool PlusJacobian(const double * x, double * jacobian) const override + { + return local_parameterization_->ComputeJacobian(x, jacobian); + } + + /** + * @brief y_minus_x = Minus(y, x) + * + * Given two points on the manifold, Minus computes the change to x in the + * tangent space at x, that will take it to y. + * + * Note that the parameter order for the Manifold class is different than + * the parameter order for the LocalParameterization class. + * + * @param[in] y is a \p AmbientSize() vector. + * @param[in] x is a \p AmbientSize() vector. + * @param[out] y_minus_x is a \p TangentSize() vector. + * @return Return value indicates if the operation was successful or not. + */ + bool Minus(const double * y, const double * x, double * y_minus_x) const override + { + return local_parameterization_->Minus(x, y, y_minus_x); + } + + /** + * @brief Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e + * + * (D_1 Minus) (y, y) + * + * @param[in] x is a \p AmbientSize() vector. + * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. + * @return Return value indicates whether the operation was successful or not. + */ + bool MinusJacobian(const double * x, double * jacobian) const override + { + return local_parameterization_->ComputeMinusJacobian(x, jacobian); + } + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + ManifoldAdapter() = default; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & local_parameterization_; + } + + /** + * @brief A legacy LocalParametrization object that will be adapted to match the Manifold interface + */ + fuse_core::LocalParameterization::SharedPtr local_parameterization_; +}; + +} // namespace fuse_core + +BOOST_CLASS_EXPORT_KEY(fuse_core::ManifoldAdapter); + +#endif + +#endif // FUSE_CORE__MANIFOLD_ADAPTER_HPP_ diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp index 218f89ae0..408bbeea0 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.hpp +++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp @@ -96,9 +96,9 @@ typename MessageBuffer::message_range MessageBuffer::query( // Find the entry that is strictly greater than the requested beginning stamp. If the extended // range flag is true, we will then back up one entry. auto upper_bound_comparison = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; + { + return element.first > stamp; + }; auto beginning_iter = std::upper_bound( buffer_.begin(), buffer_.end(), beginning_stamp, upper_bound_comparison); @@ -108,9 +108,9 @@ typename MessageBuffer::message_range MessageBuffer::query( // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is // false, we will back up one entry. auto lower_bound_comparison = [](const auto & element, const auto & stamp) -> bool - { - return element.first < stamp; - }; + { + return element.first < stamp; + }; auto ending_iter = std::lower_bound( buffer_.begin(), buffer_.end(), ending_stamp, lower_bound_comparison); @@ -155,9 +155,9 @@ void MessageBuffer::purgeHistory() // - at least two entries remains at all times // - the buffer covers *at least* until the expiration time. Longer is acceptable. auto is_greater = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; + { + return element.first > stamp; + }; auto expiration_iter = std::upper_bound( buffer_.begin(), buffer_.end(), expiration_time, is_greater); diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp index b8f1c60c5..8d38d644a 100644 --- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -17,7 +17,6 @@ #include - #include #include #include @@ -32,31 +31,31 @@ #define ALL_FUSE_CORE_NODE_INTERFACES \ - fuse_core::node_interfaces::Base, \ - fuse_core::node_interfaces::Clock, \ - fuse_core::node_interfaces::Graph, \ - fuse_core::node_interfaces::Logging, \ - fuse_core::node_interfaces::Parameters, \ - fuse_core::node_interfaces::Services, \ - fuse_core::node_interfaces::TimeSource, \ - fuse_core::node_interfaces::Timers, \ - fuse_core::node_interfaces::Topics, \ - fuse_core::node_interfaces::Waitables + fuse_core::node_interfaces::Base, \ + fuse_core::node_interfaces::Clock, \ + fuse_core::node_interfaces::Graph, \ + fuse_core::node_interfaces::Logging, \ + fuse_core::node_interfaces::Parameters, \ + fuse_core::node_interfaces::Services, \ + fuse_core::node_interfaces::TimeSource, \ + fuse_core::node_interfaces::Timers, \ + fuse_core::node_interfaces::Topics, \ + fuse_core::node_interfaces::Waitables namespace fuse_core { namespace node_interfaces { -typedef rclcpp::node_interfaces::NodeBaseInterface Base; -typedef rclcpp::node_interfaces::NodeClockInterface Clock; -typedef rclcpp::node_interfaces::NodeGraphInterface Graph; -typedef rclcpp::node_interfaces::NodeLoggingInterface Logging; -typedef rclcpp::node_interfaces::NodeParametersInterface Parameters; -typedef rclcpp::node_interfaces::NodeServicesInterface Services; -typedef rclcpp::node_interfaces::NodeTimeSourceInterface TimeSource; -typedef rclcpp::node_interfaces::NodeTimersInterface Timers; -typedef rclcpp::node_interfaces::NodeTopicsInterface Topics; -typedef rclcpp::node_interfaces::NodeWaitablesInterface Waitables; +using Base = rclcpp::node_interfaces::NodeBaseInterface; +using Clock = rclcpp::node_interfaces::NodeClockInterface; +using Graph = rclcpp::node_interfaces::NodeGraphInterface; +using Logging = rclcpp::node_interfaces::NodeLoggingInterface; +using Parameters = rclcpp::node_interfaces::NodeParametersInterface; +using Services = rclcpp::node_interfaces::NodeServicesInterface; +using TimeSource = rclcpp::node_interfaces::NodeTimeSourceInterface; +using Timers = rclcpp::node_interfaces::NodeTimersInterface; +using Topics = rclcpp::node_interfaces::NodeTopicsInterface; +using Waitables = rclcpp::node_interfaces::NodeWaitablesInterface; template using NodeInterfaces = ::rclcpp::node_interfaces::NodeInterfaces; diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 402604bb2..016b344b0 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -36,12 +36,15 @@ #include #include +#include #include #include #include #include #include +#include +#include #include #include @@ -59,10 +62,10 @@ * @endcode */ #define FUSE_VARIABLE_CLONE_DEFINITION(...) \ - fuse_core::Variable::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Variable::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -78,22 +81,22 @@ * @endcode */ #define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -111,17 +114,17 @@ * @endcode */ #define FUSE_VARIABLE_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -138,10 +141,10 @@ * @endcode */ #define FUSE_VARIABLE_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -158,10 +161,10 @@ * @endcode */ #define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core @@ -328,6 +331,39 @@ class Variable return nullptr; } +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a new Ceres manifold object to apply to updates of this + * variable + * + * If a manifold is not needed, a null pointer should be returned. If a local + * parameterization is needed, remember to also override the \p localSize() + * method to return the appropriate local parameterization size. + * + * The Ceres interface requires a raw pointer. Ceres will take ownership of + * the pointer and promises to properly delete the local parameterization when + * it is done. Additionally, fuse promises that the Variable object will + * outlive any generated local parameterization (i.e. the Ceres objects will + * be destroyed before the Variable objects). This guarantee may allow + * optimizations for the creation of the local parameterization objects. + * + * @return A base pointer to an instance of a derived Manifold + */ + virtual fuse_core::Manifold * manifold() const + { + // To support legacy Variable classes that still implements the localParameterization() method, + // construct a ManifoldAdapter object from the LocalParameterization pointer as the default + // action. If the Variable has been updated to use the new Manifold classes, then the Variable + // should override this method and return a pointer to the appropriate derived Manifold object. + auto local_parameterization = localParameterization(); + if (!local_parameterization) { + return nullptr; + } else { + return new fuse_core::ManifoldAdapter(local_parameterization); + } + } +#endif + /** * @brief Specifies the lower bound value of each variable dimension * diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 38a394209..1919e7f82 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -335,7 +336,6 @@ void loadSolverOptionsFromROS( solver_options.dogleg_type = fuse_core::declareCeresParam( interfaces, fuse_core::joinParameterName(ns, "dogleg_type"), solver_options.dogleg_type); - tmp_descr.description = ( "Enables the non-monotonic trust region algorithm as described by Conn, " "Gould & Toint in 'Trust Region Methods', Section 10.1"); @@ -355,7 +355,6 @@ void loadSolverOptionsFromROS( tmp_descr ); - tmp_descr.description = "Maximum number of iterations for which the solver should run"; solver_options.max_num_iterations = fuse_core::getParam( interfaces, @@ -499,7 +498,6 @@ void loadSolverOptionsFromROS( // No parameter is loaded for: std::shared_ptr linear_solver_ordering; - tmp_descr.description = "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; solver_options.use_explicit_schur_complement = fuse_core::getParam( @@ -509,10 +507,19 @@ void loadSolverOptionsFromROS( tmp_descr ); - // NOTE(CH3): Solved::Options::use_postordering was removed in: - // https://github.com/ceres-solver/ceres-solver/commit/8ba8fbb173db5a1e01feeafe875c1f04839fd97b - // - // So it is also not included here +#if !CERES_VERSION_AT_LEAST(2, 2, 0) + tmp_descr.description = ( + "In some rare cases, it is worth using a more complicated " + "reordering algorithm which has slightly better runtime " + "performance at the expense of an extra copy of the Jacobian " + "matrix. Setting use_postordering to true enables this tradeoff."); + solver_options.use_postordering = fuse_core::getParam( + interfaces, + fuse_core::joinParameterName(ns, "use_postordering"), + solver_options.use_postordering, + tmp_descr + ); +#endif tmp_descr.description = "This settings only affects the SPARSE_NORMAL_CHOLESKY solver."; solver_options.dynamic_sparsity = fuse_core::getParam( @@ -523,7 +530,6 @@ void loadSolverOptionsFromROS( ); #if CERES_VERSION_AT_LEAST(2, 0, 0) - tmp_descr.description = ( "NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " "\n" @@ -550,6 +556,43 @@ void loadSolverOptionsFromROS( ); #endif +#if CERES_VERSION_AT_LEAST(2, 2, 0) + tmp_descr.description = ( + "Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION. " + "Each iteration corresponds to one more term in the power series expansion " + "of the inverse of the Schur complement. This value controls the maximum " + "number of iterations whether it is used as a preconditioner or just to " + "initialize the solution for ITERATIVE_SCHUR."); + solver_options.max_num_spse_iterations = fuse_core::getParam( + interfaces, + fuse_core::joinParameterName(ns, "max_num_spse_iterations"), + solver_options.max_num_spse_iterations, + tmp_descr + ); + + tmp_descr.description = ( + "Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for " + "ITERATIVE_SCHUR. This option can be set true regardless of what " + "preconditioner is being used."); + solver_options.use_spse_initialization = fuse_core::getParam( + interfaces, + fuse_core::joinParameterName(ns, "use_spse_initialization"), + solver_options.use_spse_initialization, + tmp_descr + ); + + tmp_descr.description = ( + "When use_spse_initialization is true, this parameter along with " + "max_num_spse_iterations controls the number of " + "SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It " + "is not used to control the preconditioner."); + solver_options.spse_tolerance = fuse_core::getParam( + interfaces, + fuse_core::joinParameterName(ns, "spse_tolerance"), + solver_options.spse_tolerance, + tmp_descr + ); +#endif tmp_descr.description = "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; @@ -562,7 +605,6 @@ void loadSolverOptionsFromROS( // No parameter is loaded for: std::shared_ptr inner_iteration_ordering; - tmp_descr.description = ( "Once the relative decrease in the objective function due to " "inner iterations drops below inner_iteration_tolerance, the use " @@ -601,7 +643,6 @@ void loadSolverOptionsFromROS( tmp_descr ); - tmp_descr.description = ( "True means that the Jacobian is scaled by the norm of its columns before being passed to the " "linear solver. This improves the numerical conditioning of the normal equations"); @@ -707,6 +748,6 @@ void loadSolverOptionsFromROS( std::string(interfaces.get_node_base_interface()->get_namespace()) + ". Error: " + error); } -} +} // NOLINT [readability/fn_size] } // namespace fuse_core diff --git a/fuse_core/src/manifold_adapter.cpp b/fuse_core/src/manifold_adapter.cpp new file mode 100644 index 000000000..8b8b9e70f --- /dev/null +++ b/fuse_core/src/manifold_adapter.cpp @@ -0,0 +1,41 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Clearpath Robotics, 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. + */ +#include +#include + +#if CERES_SUPPORTS_MANIFOLDS +#include + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_core::ManifoldAdapter); +#endif diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index cbee6c839..d394b4be9 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -92,9 +92,9 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite // Also don't add the same constraint twice auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; + { + return constraint_uuid == added_constraint->uuid(); + }; auto added_constraints_iter = std::find_if( added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -110,9 +110,9 @@ void Transaction::removeConstraint(const UUID & constraint_uuid) // If the constraint being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; + { + return constraint_uuid == added_constraint->uuid(); + }; auto added_constraints_iter = std::find_if( added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -166,9 +166,9 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) // Also don't add the same variable twice auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; + { + return variable_uuid == added_variable->uuid(); + }; auto added_variables_iter = std::find_if( added_variables_.begin(), added_variables_.end(), is_variable_added); @@ -184,9 +184,9 @@ void Transaction::removeVariable(const UUID & variable_uuid) // If the variable being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; + { + return variable_uuid == added_variable->uuid(); + }; auto added_variables_iter = std::find_if( added_variables_.begin(), added_variables_.end(), is_variable_added); diff --git a/fuse_core/test/CMakeLists.txt b/fuse_core/test/CMakeLists.txt index 5d094e366..60fcb5491 100644 --- a/fuse_core/test/CMakeLists.txt +++ b/fuse_core/test/CMakeLists.txt @@ -29,7 +29,7 @@ target_link_libraries(test_util ${PROJECT_NAME}) ament_add_gtest(test_uuid test_uuid.cpp) target_link_libraries(test_uuid ${PROJECT_NAME}) -ament_add_gtest(test_variable test_variable.cpp) +ament_add_gtest(test_variable test_variable.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) target_link_libraries(test_variable ${PROJECT_NAME}) diff --git a/fuse_core/test/example_constraint.hpp b/fuse_core/test/example_constraint.hpp index e9cf49128..4185a7784 100644 --- a/fuse_core/test/example_constraint.hpp +++ b/fuse_core/test/example_constraint.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index 079eb17ee..95ea87a6e 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -31,12 +31,15 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_CORE__TEST_EXAMPLE_VARIABLE_H // NOLINT{build/header_guard} -#define FUSE_CORE__TEST_EXAMPLE_VARIABLE_H // NOLINT{build/header_guard} +#ifndef EXAMPLE_VARIABLE_HPP_ +#define EXAMPLE_VARIABLE_HPP_ #include #include #include +#include // NOLINT[build/include_order] + +#include #include #include #include @@ -61,6 +64,15 @@ class ExampleVariable : public fuse_core::Variable double * data() override {return &data_;} void print(std::ostream & /*stream = std::cout*/) const override {} +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: double data_; @@ -82,6 +94,143 @@ class ExampleVariable : public fuse_core::Variable } }; +/** + * @brief This is a test parameterization based on a quaternion representing a 3D rotation + */ +class LegacyParameterization : public fuse_core::LocalParameterization +{ +public: + FUSE_SMART_PTR_DEFINITIONS(LegacyParameterization) + + int GlobalSize() const override {return 4;} + + int LocalSize() const override {return 3;} + + bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + { + double q_delta[4]; + ceres::AngleAxisToQuaternion(delta, q_delta); + ceres::QuaternionProduct(x, q_delta, x_plus_delta); + return true; + } + + bool ComputeJacobian(const double * x, double * jacobian) const override + { + double x0 = x[0] / 2; + double x1 = x[1] / 2; + double x2 = x[2] / 2; + double x3 = x[3] / 2; + /* *INDENT-OFF* */ + jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT + jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + /* *INDENT-ON* */ + return true; + } + + bool Minus(const double * x, const double * y, double * y_minus_x) const override + { + double x_inverse[4]; + x_inverse[0] = x[0]; + x_inverse[1] = -x[1]; + x_inverse[2] = -x[2]; + x_inverse[3] = -x[3]; + + double q_delta[4]; + ceres::QuaternionProduct(x_inverse, y, q_delta); + ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + return true; + } + + bool ComputeMinusJacobian(const double * x, double * jacobian) const override + { + double x0 = x[0] * 2; + double x1 = x[1] * 2; + double x2 = x[2] * 2; + double x3 = x[3] * 2; + /* *INDENT-OFF* */ + jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + /* *INDENT-ON* */ + return true; + } + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + } +}; + +class LegacyVariable : public fuse_core::Variable +{ +public: + FUSE_VARIABLE_DEFINITIONS(LegacyVariable); + + LegacyVariable() + : fuse_core::Variable(fuse_core::uuid::generate()), + data_{1.0, 0.0, 0.0, 0.0} + { + } + + size_t size() const override {return 4;} + const double * data() const override {return data_;} + double * data() override {return data_;} + void print(std::ostream & /*stream = std::cout*/) const override {} + + /** + * @brief Returns the number of elements of the local parameterization space. + * + * While a quaternion has 4 parameters, a 3D rotation only has 3 degrees of freedom. Hence, the local + * parameterization space is only size 3. + */ + size_t localSize() const override {return 3u;} + + /** + * @brief Provides a Ceres local parameterization for the quaternion + * + * @return A pointer to a local parameterization object that indicates how to "add" increments to the quaternion + */ + fuse_core::LocalParameterization * localParameterization() const override + { + return new LegacyParameterization(); + } + +private: + double data_[4]; + + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & data_; + } +}; + BOOST_CLASS_EXPORT(ExampleVariable); +BOOST_CLASS_EXPORT(LegacyParameterization); +BOOST_CLASS_EXPORT(LegacyVariable); -#endif // FUSE_CORE__TEST_EXAMPLE_VARIABLE_H // NOLINT{build/header_guard} +#endif // EXAMPLE_VARIABLE_HPP_ diff --git a/fuse_core/test/launch_tests/test_parameters.py b/fuse_core/test/launch_tests/test_parameters.py index 9690362f2..10a000721 100755 --- a/fuse_core/test/launch_tests/test_parameters.py +++ b/fuse_core/test/launch_tests/test_parameters.py @@ -45,4 +45,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=10) - assert test_proc.return_code == 0, "GTests failed" + assert test_proc.return_code == 0, 'GTests failed' diff --git a/fuse_core/test/legacy_variable_deserialization.txt b/fuse_core/test/legacy_variable_deserialization.txt new file mode 100644 index 000000000..a8bdd3392 --- /dev/null +++ b/fuse_core/test/legacy_variable_deserialization.txt @@ -0,0 +1,2 @@ +22 serialization::archive 17 1 0 +0 0 0 5a998fb5-e259-45ca-8e88-957fd8c59a60 4 9.51999999999999957e-01 3.79999999999999991e-02 -1.89000000000000001e-01 2.38999999999999990e-01 diff --git a/fuse_core/test/test_local_parameterization.cpp b/fuse_core/test/test_local_parameterization.cpp index 5c08030d0..29273ed58 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -33,10 +33,13 @@ */ #include -#include +#include #include #include +#if !CERES_SUPPORTS_MANIFOLDS +#include + struct Plus { template @@ -52,10 +55,10 @@ struct Plus struct Minus { template - bool operator()(const T * x1, const T * x2, T * delta) const + bool operator()(const T * x, const T * y, T * y_minus_x) const { - delta[0] = (x2[0] - x1[0]) / 2.0; - delta[1] = (x2[1] - x1[1]) / 5.0; + y_minus_x[0] = (y[0] - x[0]) / 2.0; + y_minus_x[1] = (y[1] - x[1]) / 5.0; return true; } }; @@ -130,3 +133,36 @@ TEST(LocalParameterization, MinusJacobian) EXPECT_TRUE(success); EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5); } + +TEST(LocalParameterization, MinusSameVariablesIsZero) +{ + TestLocalParameterization parameterization; + + double x1[3] = {1.0, 2.0, 3.0}; + double actual[2] = {0.0, 0.0}; + bool success = parameterization.Minus(x1, x1, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(0.0, actual[0], 1.0e-5); + EXPECT_NEAR(0.0, actual[1], 1.0e-5); +} + +TEST(LocalParameterization, PlusMinus) +{ + TestLocalParameterization parameterization; + + const double x1[3] = {1.0, 2.0, 3.0}; + const double delta[2] = {0.5, 1.0}; + double x2[3] = {0.0, 0.0, 0.0}; + bool success = parameterization.Plus(x1, delta, x2); + + ASSERT_TRUE(success); + + double actual[2] = {0.0, 0.0}; + success = parameterization.Minus(x1, x2, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(delta[0], actual[0], 1.0e-5); + EXPECT_NEAR(delta[1], actual[1], 1.0e-5); +} +#endif diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index 0c9893472..d4c8caa9e 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -31,12 +31,169 @@ * 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 "example_variable.hpp" -TEST(Variable, Type) -{ +TEST(Variable, Type) { ExampleVariable variable; ASSERT_EQ("ExampleVariable", variable.type()); } + +TEST(LegacyVariable, Serialization) { + // Create an Orientation3DStamped + LegacyVariable expected; + expected.data()[0] = 0.952; + expected.data()[1] = 0.038; + expected.data()[2] = -0.189; + expected.data()[3] = 0.239; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + LegacyVariable actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.data()[0], actual.data()[0]); + EXPECT_EQ(expected.data()[1], actual.data()[1]); + EXPECT_EQ(expected.data()[2], actual.data()[2]); + EXPECT_EQ(expected.data()[3], actual.data()[3]); +} + +#if CERES_SUPPORTS_MANIFOLDS +struct QuaternionCostFunction +{ + explicit QuaternionCostFunction(double * observation) + { + observation_[0] = observation[0]; + observation_[1] = observation[1]; + observation_[2] = observation[2]; + observation_[3] = observation[3]; + } + + template + bool operator()(const T * quaternion, T * residual) const + { + T inverse_quaternion[4] = + { + quaternion[0], + -quaternion[1], + -quaternion[2], + -quaternion[3] + }; + + T observation[4] = + { + T(observation_[0]), + T(observation_[1]), + T(observation_[2]), + T(observation_[3]) + }; + + T output[4]; + + ceres::QuaternionProduct(observation, inverse_quaternion, output); + + // Residual can just be the imaginary components + residual[0] = output[1]; + residual[1] = output[2]; + residual[2] = output[3]; + + return true; + } + + double observation_[4]; +}; + +TEST(LegacyVariable, ManifoldAdapter) { + // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees + LegacyVariable orientation; + orientation.data()[0] = 0.952; + orientation.data()[1] = 0.038; + orientation.data()[2] = -0.189; + orientation.data()[3] = 0.239; + + // Create a simple a constraint with an identity quaternion + double target_quat[4] = {1.0, 0.0, 0.0, 0.0}; + ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction(new QuaternionCostFunction(target_quat)); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + std::vector parameter_blocks; + parameter_blocks.push_back(orientation.data()); + problem.AddResidualBlock( + cost_function, + nullptr, + parameter_blocks); + + // Run the solver + ceres::Solver::Summary summary; + ceres::Solver::Options options; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(target_quat[0], orientation.data()[0], 1.0e-3); + EXPECT_NEAR(target_quat[1], orientation.data()[1], 1.0e-3); + EXPECT_NEAR(target_quat[2], orientation.data()[2], 1.0e-3); + EXPECT_NEAR(target_quat[3], orientation.data()[3], 1.0e-3); +} + +TEST(LegacyVariable, Deserialization) { + // Test loading a LegacyVariable that was serialized without manifold support. + // Verify the deserialization works, and that a manifold pointer can be generated. + + LegacyVariable expected; + expected.data()[0] = 0.952; + expected.data()[1] = 0.038; + expected.data()[2] = -0.189; + expected.data()[3] = 0.239; + + // The LegacyVariable was serialized from an old version of fuse using the following code + // { + // std::ofstream output_file("legacy_variable_deserialization.txt"); + // fuse_core::TextOutputArchive archive(output_file); + // expected.serialize(archive); + // } + + // Deserialize a new variable from the previously serialzied file + LegacyVariable actual; + { + std::ifstream input_file("legacy_variable_deserialization.txt"); + fuse_core::TextInputArchive archive(input_file); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.data()[0], actual.data()[0]); + EXPECT_EQ(expected.data()[1], actual.data()[1]); + EXPECT_EQ(expected.data()[2], actual.data()[2]); + EXPECT_EQ(expected.data()[3], actual.data()[3]); + + // Test the manifold interface, and that the Legacy LocalParameterization is wrapped + // in a ManifoldAdapter + fuse_core::Manifold * actual_manifold = nullptr; + ASSERT_NO_THROW(actual_manifold = actual.manifold()); + ASSERT_NE(actual_manifold, nullptr); + auto actual_manifold_adapter = dynamic_cast(actual_manifold); + ASSERT_NE(actual_manifold_adapter, nullptr); +} +#endif diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index d1e70b173..9a0bc4dc5 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -23,6 +23,28 @@ include(boost-extras.cmake) ########### ## Build ## ########### +# lint_cmake: -linelength +# Disable warnings about maybe uninitialized variables with -Wno-maybe-uninitialized until we fix the following error: +# +# In file included from include/c++/12.2.0/functional:59, +# from include/eigen3/Eigen/Core:85, +# from include/fuse_core/fuse_macros.h:63, +# from include/fuse_core/loss.h:37, +# from include/fuse_core/constraint.h:37, +# from src/fuse/fuse_graphs/include/fuse_graphs/hash_graph.h:38, +# from src/fuse/fuse_graphs/src/hash_graph.cpp:34: +# In copy constructor ‘std::function<_Res(_ArgTypes ...)>::function(const std::function<_Res(_ArgTypes ...)>&) [with _Res = const fuse_core::Variable&; _ArgTypes = {const std::pair >&}]’, +# inlined from ‘boost::iterators::transform_iterator::transform_iterator(const Iterator&, UnaryFunc) [with UnaryFunc = std::function >&)>; Iterator = std::__detail::_Node_const_iterator >, false, true>; Reference = boost::use_default; Value = boost::use_default]’ at include/boost/iterator/transform_iterator.hpp:96:21, +# inlined from ‘boost::iterators::transform_iterator boost::iterators::make_transform_iterator(Iterator, UnaryFunc) [with UnaryFunc = std::function >&)>; Iterator = std::__detail::_Node_const_iterator >, false, true>]’ at include/boost/iterator/transform_iterator.hpp:141:61, +# inlined from ‘virtual fuse_core::Graph::const_variable_range fuse_graphs::HashGraph::getVariables() const’ at fuse/fuse_graphs/src/hash_graph.cpp:284:35: +# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:391:17: error: ‘’ may be used uninitialized [-Werror=maybe-uninitialized] +# 391 | __x._M_manager(_M_functor, __x._M_functor, __clone_functor); +# | ~~~~^~~~~~~~~~ +# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h: In member function ‘virtual fuse_core::Graph::const_variable_range fuse_graphs::HashGraph::getVariables() const’: +# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:267:7: note: by argument 2 of type ‘const std::_Any_data&’ to ‘static bool std::_Function_handler<_Res(_ArgTypes ...), _Functor>::_M_manager(std::_Any_data&, const std::_Any_data&, std::_Manager_operation) [with _Res = const fuse_core::Variable&; _Functor = fuse_graphs::HashGraph::getVariables() const::, boost::hash >::value_type&)>; _ArgTypes = {const std::pair >&}]’ declared here +# 267 | _M_manager(_Any_data& __dest, const _Any_data& __source, +# | ^~~~~~~~~~ +add_compile_options(-Wall -Werror -Wno-maybe-uninitialized) ## fuse_graphs library add_library(${PROJECT_NAME} diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.hpp b/fuse_graphs/include/fuse_graphs/hash_graph.hpp index e723edb44..8eb1b8f10 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph.hpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -458,7 +459,15 @@ void serialize( archive & options.cost_function_ownership; archive & options.disable_all_safety_checks; archive & options.enable_fast_removal; +#if !CERES_SUPPORTS_MANIFOLDS archive & options.local_parameterization_ownership; +#else + // Local parameterizations got marked as deprecated in favour of Manifold in version 2.1.0, see + // https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc + // and they got removed in 2.2.0, see + // https://github.com/ceres-solver/ceres-solver/commit/68c53bb39552cd4abfd6381df08638285f7386b3 + archive & options.manifold_ownership; +#endif archive & options.loss_function_ownership; } diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 836f55097..412c21696 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -319,11 +320,11 @@ void HashGraph::getCovariance( // covariance matrix is symmetric, requesting Cov(A,B) and Cov(B,A) counts as a duplicate. Create // an expression to test a pair of data pointers such that (A,B) == (A,B) OR (B,A) auto symmetric_equal = [](const std::pair & x, - const std::pair & y) - { - return ((x.first == y.first) && (x.second == y.second)) || - ((x.first == y.second) && (x.second == y.first)); - }; + const std::pair & y) + { + return ((x.first == y.first) && (x.second == y.second)) || + ((x.first == y.second) && (x.second == y.first)); + }; // Convert the covariance requests into the input structure needed by Ceres. Namely, we must // convert the variable UUIDs into memory addresses. We create two containers of covariance // blocks: one only contains the unique variable pairs that we give to Ceres, and a second that @@ -485,7 +486,11 @@ void HashGraph::createProblem(ceres::Problem & problem) const problem.AddParameterBlock( variable.data(), variable.size(), +#if !CERES_SUPPORTS_MANIFOLDS variable.localParameterization()); +#else + variable.manifold()); +#endif // Handle optimization bounds for (size_t index = 0; index < variable.size(); ++index) { auto lower_bound = variable.lowerBound(index); diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp index 8a3ca5d4f..52f9a9302 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp @@ -115,9 +115,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase std::to_string(sigma_vector.size())); } auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; + { + return std::isfinite(sigma) && (sigma > 0); + }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { throw std::invalid_argument( "The supplied initial_sigma parameter must contain valid floating point values. " @@ -139,9 +139,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase std::to_string(state_vector.size())); } auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; + { + return std::isfinite(state); + }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { throw std::invalid_argument( "The supplied initial_state parameter must contain valid floating point values. " diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index fcaef9dc7..ac3202d40 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -97,13 +97,7 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; - -#if CERES_VERSION_AT_LEAST(2, 1, 0) - std::vector parameterizations; - ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); -#else ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); -#endif // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error // is 5.26356e-10 diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f8d85f09a..096928a2a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -185,13 +185,13 @@ void FixedLagSmoother::postprocessMarginalization( void FixedLagSmoother::optimizationLoop() { auto exit_wait_condition = [this]() - { - return - this->optimization_request_ || - !this->optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid() - ; - }; + { + return + this->optimization_request_ || + !this->optimization_running_ || + !interfaces_.get_node_base_interface()->get_context()->is_valid() + ; + }; // Optimize constraints until told to exit while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) @@ -518,9 +518,9 @@ void FixedLagSmoother::transactionCallback( // The pending set is arranged "smallest stamp last" to making popping off the back more // efficient auto comparator = [](const rclcpp::Time & value, const TransactionQueueElement & element) - { - return value >= element.stamp(); - }; + { + return value >= element.stamp(); + }; auto position = std::upper_bound( pending_transactions_.begin(), pending_transactions_.end(), @@ -550,8 +550,8 @@ void FixedLagSmoother::transactionCallback( [&sensor_name, max_time, & min_time = start_time](const auto & transaction) { // NOLINT(whitespace/braces) return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) pending_transactions_.end()); } else { // And purge out old transactions to limit the pending size while waiting for an ignition diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 2a4b62647..16c3cfa09 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -501,9 +501,10 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists"); - auto print_key = [](const std::string & result, const auto & entry) { - return result + entry.first + ' '; - }; + auto print_key = [](const std::string & result, const auto & entry) + { + return result + entry.first + ' '; + }; status.add( "Sensor Models", diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index 60038053d..2a5b71775 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -47,9 +47,9 @@ namespace fuse_optimizers rclcpp::Time VariableStampIndex::currentStamp() const { auto compare_stamps = [](const StampedMap::value_type & lhs, const StampedMap::value_type & rhs) - { - return lhs.second < rhs.second; - }; + { + return lhs.second < rhs.second; + }; auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); if (iter != stamped_index_.end()) { return iter->second; diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp index 50ff48f0b..2895bc8d2 100644 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp @@ -76,7 +76,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) auto relative_pose_publisher = node->create_publisher( - "/relative_pose", 1); + "/relative_pose", 5); // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify. ASSERT_TRUE(node->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py index d171d15de..da1a501c6 100755 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py @@ -18,12 +18,12 @@ from launch import LaunchDescription from launch.actions import ExecuteProcess -from launch_ros.actions import Node from launch.substitutions import PathJoinSubstitution import launch_pytest from launch_pytest.actions import ReadyToTest from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node import pytest @@ -62,4 +62,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=30) - assert test_proc.return_code == 0, "GTests failed" + assert test_proc.return_code == 0, 'GTests failed' diff --git a/fuse_optimizers/test/launch_tests/test_optimizer.py b/fuse_optimizers/test/launch_tests/test_optimizer.py index 989b2eb0e..e1a88a8ad 100755 --- a/fuse_optimizers/test/launch_tests/test_optimizer.py +++ b/fuse_optimizers/test/launch_tests/test_optimizer.py @@ -45,4 +45,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=30) - assert test_proc.return_code == 0, "GTests failed" + assert test_proc.return_code == 0, 'GTests failed' diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 72b0ce389..69c5be8f4 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -142,8 +142,7 @@ void Path2DPublisher::notifyCallback( } // Sort the poses by timestamp auto compare_stamps = - [](const geometry_msgs::msg::PoseStamped & pose1, - const geometry_msgs::msg::PoseStamped & pose2) + [](const geometry_msgs::msg::PoseStamped & pose1, const geometry_msgs::msg::PoseStamped & pose2) { if (pose1.header.stamp.sec == pose2.header.stamp.sec) { return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; diff --git a/fuse_tutorials/launch/fuse_simple_tutorial.launch.py b/fuse_tutorials/launch/fuse_simple_tutorial.launch.py index 2268baac3..46a3308e2 100755 --- a/fuse_tutorials/launch/fuse_simple_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_simple_tutorial.launch.py @@ -14,12 +14,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from launch_ros.actions import SetParameter, Node -from launch_ros.substitutions import FindPackageShare - from launch import LaunchDescription from launch.actions import ExecuteProcess from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): diff --git a/fuse_tutorials/launch/range_sensor_tutorial.launch.py b/fuse_tutorials/launch/range_sensor_tutorial.launch.py index ce708483e..2b084629e 100755 --- a/fuse_tutorials/launch/range_sensor_tutorial.launch.py +++ b/fuse_tutorials/launch/range_sensor_tutorial.launch.py @@ -14,19 +14,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): pkg_dir = FindPackageShare('fuse_tutorials') return LaunchDescription([ - Node(package="tf2_ros", - executable="static_transform_publisher", + Node(package='tf2_ros', + executable='static_transform_publisher', arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), Node( package='fuse_tutorials', diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index 669087826..bc9f1714b 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -105,6 +107,15 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index 0cbadb015..b43683cfc 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -127,6 +129,15 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index ecb96cc02..ae7a539a8 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -116,6 +118,15 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index 9d2fcc2f2..c48e45a81 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -127,6 +129,15 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index f7bcb74f6..987d64712 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -36,18 +36,20 @@ #include +#include +#include +#include + +#include +#include #include +#include #include #include #include #include #include #include - -#include -#include -#include - #include namespace fuse_variables @@ -64,6 +66,8 @@ namespace fuse_variables class Orientation2DLocalParameterization : public fuse_core::LocalParameterization { public: + FUSE_SMART_PTR_DEFINITIONS(Orientation2DLocalParameterization) + int GlobalSize() const override { return 1; @@ -93,12 +97,12 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati } bool Minus( - const double * x1, - const double * x2, - double * delta) const override + const double * x, + const double * y, + double * y_minus_x) const override { - // Compute the difference from x2 to x1, and handle the 2*Pi rollover - delta[0] = fuse_core::wrapAngle2D(x2[0] - x1[0]); + // Compute the difference from x to y, and handle the 2*Pi rollover + y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } @@ -128,6 +132,67 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati } }; +#if CERES_SUPPORTS_MANIFOLDS +/** + * @brief A Manifold class for 2D Orientations. + * + * 2D orientations add and subtract in the "usual" way, except for the 2*pi rollover issue. This local parameterization + * handles the rollover. Because the Jacobians for this parameterization are always identity, we implement this + * parameterization with "analytic" derivatives, instead of using the Ceres's autodiff system. + */ +class Orientation2DManifold : public fuse_core::Manifold +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Orientation2DManifold) + + int AmbientSize() const override {return 1;} + + int TangentSize() const override {return 1;} + + bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + { + // Compute the angle increment as a linear update, and handle the 2*Pi rollover + x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); + return true; + } + + bool PlusJacobian(const double * /*x*/, double * jacobian) const override + { + jacobian[0] = 1.0; + return true; + } + + bool Minus(const double * y, const double * x, double * y_minus_x) const override + { + // Compute the difference from y to x, and handle the 2*Pi rollover + y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); + return true; + } + + bool MinusJacobian(const double * /*x*/, double * jacobian) const override + { + jacobian[0] = 1.0; + return true; + } + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + } +}; +#endif + /** * @brief Variable representing a 2D orientation (theta) at a specific time, with a specific piece * of hardware. @@ -200,6 +265,18 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped */ fuse_core::LocalParameterization * localParameterization() const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a new Ceres manifold object to apply to updates of this variable + * + * A 2D rotation has a nonlinearity when the angle wraps around from -PI to PI. This is handled by a custom + * manifold to ensure smooth derivatives. + * + * @return A base pointer to an instance of a derived manifold + */ + fuse_core::Manifold * manifold() const override; +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -221,6 +298,9 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped } // namespace fuse_variables +#if CERES_SUPPORTS_MANIFOLDS +BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation2DManifold); +#endif BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation2DLocalParameterization); BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation2DStamped); diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 65f5810cc..9e7b0ed3b 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -38,7 +38,10 @@ #include +#include +#include #include +#include #include #include #include @@ -55,6 +58,20 @@ namespace fuse_variables { +/** + * @brief Create the inverse quaternion + * + * ceres/rotation.h is missing this function for some reason. + */ +template +inline static void QuaternionInverse(const T in[4], T out[4]) +{ + out[0] = in[0]; + out[1] = -in[1]; + out[2] = -in[2]; + out[3] = -in[3]; +} + /** * @brief A LocalParameterization class for 3D Orientations. * @@ -66,29 +83,11 @@ namespace fuse_variables class Orientation3DLocalParameterization : public fuse_core::LocalParameterization { public: - /** - * @brief Create the inverse quaternion - * - * ceres/rotation.h is missing this function for some reason. - */ - template inline - static void QuaternionInverse(const T in[4], T out[4]) - { - out[0] = in[0]; - out[1] = -in[1]; - out[2] = -in[2]; - out[3] = -in[3]; - } + FUSE_SMART_PTR_DEFINITIONS(Orientation3DLocalParameterization) - int GlobalSize() const override - { - return 4; - } + int GlobalSize() const override {return 4;} - int LocalSize() const override - { - return 3; - } + int LocalSize() const override {return 3;} bool Plus( const double * x, @@ -119,15 +118,15 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati } bool Minus( - const double * x1, - const double * x2, - double * delta) const override + const double * x, + const double * y, + double * y_minus_x) const override { - double x1_inverse[4]; - QuaternionInverse(x1, x1_inverse); + double x_inverse[4]; + QuaternionInverse(x, x_inverse); double q_delta[4]; - ceres::QuaternionProduct(x1_inverse, x2, q_delta); - ceres::QuaternionToAngleAxis(q_delta, delta); + ceres::QuaternionProduct(x_inverse, y, q_delta); + ceres::QuaternionToAngleAxis(q_delta, y_minus_x); return true; } @@ -165,6 +164,88 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati } }; +#if CERES_SUPPORTS_MANIFOLDS +/** + * @brief A Manifold class for 2D Orientations. + * + * 2D orientations add and subtract in the "usual" way, except for the 2*pi rollover issue. This local parameterization + * handles the rollover. Because the Jacobians for this parameterization are always identity, we implement this + * parameterization with "analytic" derivatives, instead of using the Ceres's autodiff system. + */ +class Orientation3DManifold : public fuse_core::Manifold +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Orientation3DManifold) + + int AmbientSize() const override {return 4;} + + int TangentSize() const override {return 3;} + + bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + { + double q_delta[4]; + ceres::AngleAxisToQuaternion(delta, q_delta); + ceres::QuaternionProduct(x, q_delta, x_plus_delta); + return true; + } + + bool PlusJacobian(const double * x, double * jacobian) const override + { + double x0 = x[0] / 2; + double x1 = x[1] / 2; + double x2 = x[2] / 2; + double x3 = x[3] / 2; + /* *INDENT-OFF* */ + jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT + jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + /* *INDENT-ON* */ + return true; + } + + bool Minus(const double * y, const double * x, double * y_minus_x) const override + { + double x_inverse[4]; + QuaternionInverse(x, x_inverse); + double q_delta[4]; + ceres::QuaternionProduct(x_inverse, y, q_delta); + ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + return true; + } + + bool MinusJacobian(const double * x, double * jacobian) const override + { + double x0 = x[0] * 2; + double x1 = x[1] * 2; + double x2 = x[2] * 2; + double x3 = x[3] * 2; + /* *INDENT-OFF* */ + jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + /* *INDENT-ON* */ + return true; + } + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @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 + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + } +}; +#endif + /** * @brief Variable representing a 3D orientation as a quaternion at a specific time and for a * specific piece of hardware (e.g., robot) @@ -296,6 +377,15 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped */ fuse_core::LocalParameterization * localParameterization() const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Provides a Ceres manifold for the quaternion + * + * @return A pointer to a manifold object that indicates how to "add" increments to the quaternion + */ + fuse_core::Manifold * manifold() const override; +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -317,6 +407,9 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped } // namespace fuse_variables +#if CERES_SUPPORTS_MANIFOLDS +BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation3DManifold); +#endif BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation3DLocalParameterization); BOOST_CLASS_EXPORT_KEY(fuse_variables::Orientation3DStamped); diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index 4abfad789..869d29943 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -36,7 +36,9 @@ #include +#include #include +#include #include #include @@ -117,6 +119,15 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> */ bool holdConstant() const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 50ae6268d..9f6673654 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -36,7 +36,9 @@ #include +#include #include +#include #include #include @@ -112,6 +114,15 @@ class Point2DLandmark : public FixedSizeVariable<2> */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index dcc76fed4..0d78b1959 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -36,7 +36,9 @@ #include +#include #include +#include #include #include @@ -128,6 +130,15 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> */ bool holdConstant() const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index fa487fd95..9b184e699 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -39,7 +39,9 @@ #include +#include #include +#include #include #include @@ -126,6 +128,15 @@ class Point3DLandmark : public FixedSizeVariable<3> */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index cbe4db79f..38496d206 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -117,6 +119,15 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index 8ffa7f39d..b2d22e5b8 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -127,6 +129,15 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 4fa36aa4f..91aa62a2b 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -105,6 +107,15 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index f055fd2d8..312e58d59 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -127,6 +129,15 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index 9aa257428..19ab2de72 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -117,6 +119,15 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index 9ffb2b939..a86c0e1cb 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -36,8 +36,10 @@ #include -#include +#include +#include #include +#include #include #include #include @@ -128,6 +130,15 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped */ void print(std::ostream & stream = std::cout) const override; +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold * manifold() const override {return nullptr;} +#endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 2de7f2d8e..3897d6f97 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -69,8 +70,18 @@ fuse_core::LocalParameterization * Orientation2DStamped::localParameterization() return new Orientation2DLocalParameterization(); } +#if CERES_SUPPORTS_MANIFOLDS +fuse_core::Manifold * Orientation2DStamped::manifold() const +{ + return new Orientation2DManifold(); +} +#endif + } // namespace fuse_variables +#if CERES_SUPPORTS_MANIFOLDS +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation2DManifold); +#endif BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation2DLocalParameterization); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation2DStamped); PLUGINLIB_EXPORT_CLASS(fuse_variables::Orientation2DStamped, fuse_core::Variable); diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index ae69af29c..4eb816a95 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -72,8 +73,18 @@ fuse_core::LocalParameterization * Orientation3DStamped::localParameterization() return new Orientation3DLocalParameterization(); } +#if CERES_SUPPORTS_MANIFOLDS +fuse_core::Manifold * Orientation3DStamped::manifold() const +{ + return new Orientation3DManifold(); +} +#endif + } // namespace fuse_variables +#if CERES_SUPPORTS_MANIFOLDS +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation3DManifold); +#endif BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation3DLocalParameterization); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Orientation3DStamped); PLUGINLIB_EXPORT_CLASS(fuse_variables::Orientation3DStamped, fuse_core::Variable); diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index 086c3741c..c6f527ee8 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -127,8 +127,7 @@ TEST(AccelerationAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - acceleration.data(), acceleration.size(), acceleration.localParameterization()); + problem.AddParameterBlock(acceleration.data(), acceleration.size()); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock( diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index 957547399..39d416fc9 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -131,9 +131,7 @@ TEST(AccelerationAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - acceleration.data(), - acceleration.size(), acceleration.localParameterization()); + problem.AddParameterBlock(acceleration.data(), acceleration.size()); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index 40bd34f05..a1b4212e8 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -129,8 +129,7 @@ TEST(AccelerationLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - acceleration.data(), acceleration.size(), acceleration.localParameterization()); + problem.AddParameterBlock(acceleration.data(), acceleration.size()); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index bbe9bbbbb..83ac4093c 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -131,8 +131,7 @@ TEST(AccelerationLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - acceleration.data(), acceleration.size(), acceleration.localParameterization()); + problem.AddParameterBlock(acceleration.data(), acceleration.size()); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index dabdc7fc1..8e17d43bc 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -118,9 +119,9 @@ struct Orientation2DPlus struct Orientation2DMinus { template - bool operator()(const T * x1, const T * x2, T * delta) const + bool operator()(const T * x, const T * y, T * y_minus_x) const { - delta[0] = fuse_core::wrapAngle2D(x2[0] - x1[0]); + y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } }; @@ -250,8 +251,17 @@ TEST(Orientation2DStamped, Optimization) // Build the problem. ceres::Problem problem; +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock( - orientation.data(), orientation.size(), orientation.localParameterization()); + orientation.data(), + orientation.size(), + orientation.localParameterization()); +#else + problem.AddParameterBlock( + orientation.data(), + orientation.size(), + orientation.manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -291,3 +301,124 @@ TEST(Orientation2DStamped, Serialization) EXPECT_EQ(expected.stamp(), actual.stamp()); EXPECT_EQ(expected.yaw(), actual.yaw()); } + +#if CERES_SUPPORTS_MANIFOLDS +#include + +struct Orientation2DFunctor +{ + template + bool Plus(const T * x, const T * delta, T * x_plus_delta) const + { + x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); + return true; + } + + template + bool Minus(const T * y, const T * x, T * y_minus_x) const + { + y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); + return true; + } +}; + +using Orientation2DManifold = ceres::AutoDiffManifold; + +TEST(Orientation2DStamped, ManifoldPlus) +{ + auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + + // Simple test + { + double x[1] = {1.0}; + double delta[1] = {0.5}; + double actual[1] = {0.0}; + bool success = manifold->Plus(x, delta, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(1.5, actual[0], 1.0e-5); + } + + // Check roll-over + { + double x[1] = {2.0}; + double delta[1] = {3.0}; + double actual[1] = {0.0}; + bool success = manifold->Plus(x, delta, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); + } + + delete manifold; +} + +TEST(Orientation2DStamped, ManifoldPlusJacobian) +{ + auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto reference = Orientation2DManifold(); + + auto test_values = std::vector {-2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI}; + for (auto test_value : test_values) { + double x[1] = {test_value}; + double actual[1] = {0.0}; + bool success = manifold->PlusJacobian(x, actual); + + double expected[1] = {0.0}; + reference.PlusJacobian(x, expected); + + EXPECT_TRUE(success); + EXPECT_NEAR(expected[0], actual[0], 1.0e-5); + } + + delete manifold; +} + +TEST(Orientation2DStamped, ManifoldMinus) +{ + auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + + // Simple test + { + double x1[1] = {1.0}; + double x2[1] = {1.5}; + double actual[1] = {0.0}; + bool success = manifold->Minus(x2, x1, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(0.5, actual[0], 1.0e-5); + } + + // Check roll-over + { + double x1[1] = {2.0}; + double x2[1] = {5 - 2 * M_PI}; + double actual[1] = {0.0}; + bool success = manifold->Minus(x2, x1, actual); + + EXPECT_TRUE(success); + EXPECT_NEAR(3.0, actual[0], 1.0e-5); + } +} + +TEST(Orientation2DStamped, ManifoldMinusJacobian) +{ + auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto reference = Orientation2DManifold(); + + auto test_values = std::vector {-2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI}; + for (auto test_value : test_values) { + double x[1] = {test_value}; + double actual[1] = {0.0}; + bool success = manifold->MinusJacobian(x, actual); + + double expected[1] = {0.0}; + reference.MinusJacobian(x, expected); + + EXPECT_TRUE(success); + EXPECT_NEAR(expected[0], actual[0], 1.0e-5); + } + + delete manifold; +} +#endif diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 2e8d07dd7..10e04d0b8 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -109,6 +110,15 @@ TEST(Orientation3DStamped, UUID) } } +template +inline static void QuaternionInverse(const T in[4], T out[4]) +{ + out[0] = in[0]; + out[1] = -in[1]; + out[2] = -in[2]; + out[3] = -in[3]; +} + struct Orientation3DPlus { template @@ -124,16 +134,13 @@ struct Orientation3DPlus struct Orientation3DMinus { template - bool operator()(const T * q1, const T * q2, T * delta) const + bool operator()(const T * x, const T * y, T * y_minus_x) const { - T q1_inverse[4]; - q1_inverse[0] = q1[0]; - q1_inverse[1] = -q1[1]; - q1_inverse[2] = -q1[2]; - q1_inverse[3] = -q1[3]; + T x_inverse[4]; + QuaternionInverse(x, x_inverse); T q_delta[4]; - ceres::QuaternionProduct(q1_inverse, q2, q_delta); - ceres::QuaternionToAngleAxis(q_delta, delta); + ceres::QuaternionProduct(x_inverse, y, q_delta); + ceres::QuaternionToAngleAxis(q_delta, y_minus_x); return true; } }; @@ -342,8 +349,17 @@ TEST(Orientation3DStamped, Optimization) // Build the problem. ceres::Problem problem; +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock( - orientation.data(), orientation.size(), orientation.localParameterization()); + orientation.data(), + orientation.size(), + orientation.localParameterization()); +#else + problem.AddParameterBlock( + orientation.data(), + orientation.size(), + orientation.manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -421,3 +437,187 @@ TEST(Orientation3DStamped, Serialization) EXPECT_EQ(expected.y(), actual.y()); EXPECT_EQ(expected.z(), actual.z()); } + +#if CERES_SUPPORTS_MANIFOLDS +#include + +struct Orientation3DFunctor +{ + template + bool Plus(const T * x, const T * delta, T * x_plus_delta) const + { + T q_delta[4]; + ceres::AngleAxisToQuaternion(delta, q_delta); + ceres::QuaternionProduct(x, q_delta, x_plus_delta); + return true; + } + template + bool Minus(const T * y, const T * x, T * y_minus_x) const + { + T x_inverse[4]; + QuaternionInverse(x, x_inverse); + T q_delta[4]; + ceres::QuaternionProduct(x_inverse, y, q_delta); + ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + return true; + } +}; + +using Orientation3DManifold = ceres::AutoDiffManifold; + +TEST(Orientation3DStamped, ManifoldPlus) +{ + auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + + double x[4] = {0.842614977, 0.2, 0.3, 0.4}; + double delta[3] = {0.15, -0.2, 0.433012702}; + double result[4] = {0.0, 0.0, 0.0, 0.0}; + bool success = manifold->Plus(x, delta, result); + + EXPECT_TRUE(success); + EXPECT_NEAR(0.745561, result[0], 1.0e-5); + EXPECT_NEAR(0.360184, result[1], 1.0e-5); + EXPECT_NEAR(0.194124, result[2], 1.0e-5); + EXPECT_NEAR(0.526043, result[3], 1.0e-5); + + delete manifold; +} + +TEST(Orientation3DStamped, ManifoldPlusJacobian) +{ + auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto reference = Orientation3DManifold(); + + for (double qx = -0.5; qx < 0.5; qx += 0.1) { + for (double qy = -0.5; qy < 0.5; qy += 0.1) { + for (double qz = -0.5; qz < 0.5; qz += 0.1) { + double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + + double x[4] = {qw, qx, qy, qz}; + fuse_core::MatrixXd actual(4, 3); + /* *INDENT-OFF* */ + actual << 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0; + /* *INDENT-ON* */ + bool success = manifold->PlusJacobian(x, actual.data()); + + fuse_core::MatrixXd expected(4, 3); + /* *INDENT-OFF* */ + expected << 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0; + /* *INDENT-ON* */ + reference.PlusJacobian(x, expected.data()); + + EXPECT_TRUE(success); + Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + EXPECT_TRUE( + expected.isApprox( + actual, + 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" + << "Actual is:\n" << actual.format(clean) << "\n" + << "Difference is:\n" << (expected - actual).format(clean) + << "\n"; + } + } + } + + delete manifold; +} + +TEST(Orientation3DStamped, ManifoldMinus) +{ + double x1[4] = {0.842614977, 0.2, 0.3, 0.4}; + double x2[4] = {0.745561, 0.360184, 0.194124, 0.526043}; + double result[3] = {0.0, 0.0, 0.0}; + + auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + bool success = manifold->Minus(x2, x1, result); + + EXPECT_TRUE(success); + EXPECT_NEAR(0.15, result[0], 1.0e-5); + EXPECT_NEAR(-0.2, result[1], 1.0e-5); + EXPECT_NEAR(0.433012702, result[2], 1.0e-5); + + delete manifold; +} + +TEST(Orientation3DStamped, ManifoldMinusJacobian) +{ + auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto reference = Orientation3DManifold(); + + for (double qx = -0.5; qx < 0.5; qx += 0.1) { + for (double qy = -0.5; qy < 0.5; qy += 0.1) { + for (double qz = -0.5; qz < 0.5; qz += 0.1) { + double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + + double x[4] = {qw, qx, qy, qz}; + fuse_core::MatrixXd actual(3, 4); + /* *INDENT-OFF* */ + actual << 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0; + /* *INDENT-ON* */ + bool success = manifold->MinusJacobian(x, actual.data()); + + fuse_core::MatrixXd expected(3, 4); + /* *INDENT-OFF* */ + expected << 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0; + /* *INDENT-ON* */ + reference.MinusJacobian(x, expected.data()); + + EXPECT_TRUE(success); + Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + EXPECT_TRUE( + expected.isApprox( + actual, + 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" + << "Actual is:\n" << actual.format(clean) << "\n" + << "Difference is:\n" << (expected - actual).format(clean) + << "\n"; + } + } + } + + delete manifold; +} + +TEST(Orientation3DStamped, ManifoldSerialization) +{ + // Create an Orientation3DStamped + Orientation3DStamped expected(rclcpp::Time(12345678, 910111213)); + expected.w() = 0.952; + expected.x() = 0.038; + expected.y() = -0.189; + expected.z() = 0.239; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + Orientation3DStamped actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.deviceId(), actual.deviceId()); + EXPECT_EQ(expected.stamp(), actual.stamp()); + EXPECT_EQ(expected.w(), actual.w()); + EXPECT_EQ(expected.x(), actual.x()); + EXPECT_EQ(expected.y(), actual.y()); + EXPECT_EQ(expected.z(), actual.z()); +} + +#endif diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 114cf6bd7..9aa369adf 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -78,7 +78,6 @@ struct CostFunctor { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); - residual[2] = x[2] - T(3.1); return true; } }; diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index 771e603d1..584ea8ef4 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -129,7 +129,7 @@ TEST(Position2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size(), position.localParameterization()); + problem.AddParameterBlock(position.data(), position.size()); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index ce259f485..112943f2a 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -146,9 +146,7 @@ TEST(Position3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - position.data(), - position.size()); + problem.AddParameterBlock(position.data(), position.size()); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock( diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index a360427e2..bae0a296a 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -127,7 +127,7 @@ TEST(VelocityAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size(), velocity.localParameterization()); + problem.AddParameterBlock(velocity.data(), velocity.size()); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index ffe0e90f8..d1378aad1 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -132,10 +132,7 @@ TEST(VelocityAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - velocity.data(), - velocity.size(), - velocity.localParameterization()); + problem.AddParameterBlock(velocity.data(), velocity.size()); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock( diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index 5f5cc3b13..914b13d32 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -129,10 +129,7 @@ TEST(VelocityLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - velocity.data(), - velocity.size(), - velocity.localParameterization()); + problem.AddParameterBlock(velocity.data(), velocity.size()); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock( diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 03501c744..59e6909d0 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -132,10 +132,7 @@ TEST(VelocityLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock( - velocity.data(), - velocity.size(), - velocity.localParameterization()); + problem.AddParameterBlock(velocity.data(), velocity.size()); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock( diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt index 847e3c223..8f4f2bfb2 100644 --- a/fuse_viz/CMakeLists.txt +++ b/fuse_viz/CMakeLists.txt @@ -52,9 +52,10 @@ target_link_libraries(${PROJECT_NAME} PRIVATE fuse_constraints::fuse_constraints fuse_core::fuse_core fuse_variables::fuse_variables + ${geometry_msgs_TARGETS} rviz_common::rviz_common rviz_rendering::rviz_rendering - tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp index 96418d678..7b3381d98 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp @@ -302,7 +302,8 @@ void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha( void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale( const VisualPtr & constraint) { - constraint->setRelativePoseAxesScale(Ogre::Vector3{relative_pose_axes_scale_property_->getFloat()}); // NOLINT + constraint->setRelativePoseAxesScale( + Ogre::Vector3{relative_pose_axes_scale_property_->getFloat()}); } void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha( From b21c1698ce4669333d37ebc7ac641b52a4b4f752 Mon Sep 17 00:00:00 2001 From: giacomo Date: Tue, 22 Oct 2024 17:15:26 +0200 Subject: [PATCH 112/116] uncrustify --- .../fuse_constraints/absolute_constraint.hpp | 2 +- ...olute_pose_3d_stamped_euler_constraint.hpp | 4 +- ...ormal_delta_pose_3d_euler_cost_functor.hpp | 20 +- .../fuse_constraints/normal_prior_pose_3d.hpp | 2 +- .../normal_prior_pose_3d_euler.hpp | 2 +- ...ative_pose_3d_stamped_euler_constraint.hpp | 4 +- fuse_constraints/src/absolute_constraint.cpp | 2 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 26 +-- .../src/marginalize_variables.cpp | 2 +- .../src/normal_prior_orientation_3d.cpp | 8 +- fuse_constraints/src/normal_prior_pose_3d.cpp | 8 +- .../src/normal_prior_pose_3d_euler.cpp | 8 +- ...ative_pose_3d_stamped_euler_constraint.cpp | 14 +- fuse_constraints/src/variable_constraints.cpp | 6 +- ...rientation_3d_stamped_euler_constraint.cpp | 4 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 8 +- .../test/test_normal_prior_pose_3d.cpp | 21 +- .../test/test_normal_prior_pose_3d_euler.cpp | 40 ++-- ...ative_pose_3d_stamped_euler_constraint.cpp | 20 +- .../include/fuse_core/callback_wrapper.hpp | 2 +- fuse_core/include/fuse_core/ceres_options.hpp | 28 +-- fuse_core/include/fuse_core/constraint.hpp | 78 ++++---- fuse_core/include/fuse_core/eigen_gtest.hpp | 34 ++-- fuse_core/include/fuse_core/fuse_macros.hpp | 92 ++++----- fuse_core/include/fuse_core/graph.hpp | 60 +++--- fuse_core/include/fuse_core/loss.hpp | 70 +++---- fuse_core/include/fuse_core/macros.hpp | 92 ++++----- .../include/fuse_core/message_buffer_impl.hpp | 18 +- .../node_interfaces/node_interfaces.hpp | 20 +- fuse_core/include/fuse_core/variable.hpp | 78 ++++---- fuse_core/src/transaction.cpp | 24 +-- fuse_graphs/src/hash_graph.cpp | 10 +- .../fuse_models/common/sensor_config.hpp | 6 +- .../fuse_models/common/sensor_proc.hpp | 150 ++++++++------ .../fuse_models/omnidirectional_3d.hpp | 10 +- .../omnidirectional_3d_ignition.hpp | 16 +- .../omnidirectional_3d_predict.hpp | 146 ++++++++------ ...omnidirectional_3d_state_cost_function.hpp | 17 +- .../omnidirectional_3d_state_cost_functor.hpp | 6 +- ...ectional_3d_state_kinematic_constraint.hpp | 6 +- .../fuse_models/parameters/imu_3d_params.hpp | 4 +- .../parameters/odometry_3d_params.hpp | 2 +- .../omnidirectional_3d_ignition_params.hpp | 14 +- .../unicycle_2d_ignition_params.hpp | 12 +- fuse_models/src/imu_3d.cpp | 8 +- fuse_models/src/odometry_3d.cpp | 30 +-- fuse_models/src/odometry_3d_publisher.cpp | 32 +-- fuse_models/src/omnidirectional_3d.cpp | 44 ++-- .../src/omnidirectional_3d_ignition.cpp | 59 +++--- ...ectional_3d_state_kinematic_constraint.cpp | 4 +- fuse_models/test/test_omnidirectional_3d.cpp | 31 +-- .../test/test_omnidirectional_3d_ignition.cpp | 27 +-- .../test/test_omnidirectional_3d_predict.cpp | 188 +++++++++--------- ...omnidirectional_3d_state_cost_function.cpp | 17 +- fuse_models/test/test_sensor_proc_3d.cpp | 19 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 22 +- fuse_optimizers/src/optimizer.cpp | 6 +- fuse_optimizers/src/variable_stamp_index.cpp | 6 +- .../src/three_dimensional_simulator.cpp | 64 +++--- 59 files changed, 931 insertions(+), 822 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index b9e38f4d7..995e0e8ea 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -204,7 +204,7 @@ using AbsoluteAccelerationAngular3DStampedConstraint = AbsoluteConstraint; using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; -using AbsoluteAccelerationLinear3DStampedConstraint = +using AbsoluteAccelerationLinear3DStampedConstraint = AbsoluteConstraint; using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index 63c2d8502..672b52dac 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -60,7 +60,7 @@ namespace fuse_constraints /** * @brief A constraint that represents either prior information about a 3D pose, or a direct - * measurement of the 3D pose. + * measurement of the 3D pose. * * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, * this class applies an absolute constraint on both variables at once. This type of constraint @@ -98,7 +98,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation, const fuse_core::Vector6d & mean, const fuse_core::Matrix6d & covariance); - + /** * @brief Create a constraint using a partial measurement/prior of the 3D pose * diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index afb16217d..6ac3eb1c2 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -117,7 +117,7 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( T orientation_delta[4]; T orientation_delta_rpy[3]; - T orientation1_inverse[4] + T orientation1_inverse[4] { orientation1[0], -orientation1[1], @@ -125,13 +125,13 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( -orientation1[3] }; - T position_delta[3] + T position_delta[3] { position2[0] - position1[0], position2[1] - position1[1], position2[2] - position1[2] }; - + // Compute the position residual ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); full_residuals[0] = position_delta_rotated[0] - T(b_(0)); @@ -140,9 +140,15 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( // Compute the orientation residual ceres::QuaternionProduct(orientation1_inverse, orientation2, orientation_delta); - orientation_delta_rpy[0] = fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[1] = fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[2] = fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[0] = fuse_core::getRoll( + orientation_delta[0], orientation_delta[1], + orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[1] = fuse_core::getPitch( + orientation_delta[0], orientation_delta[1], + orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[2] = fuse_core::getYaw( + orientation_delta[0], orientation_delta[1], + orientation_delta[2], orientation_delta[3]); full_residuals[3] = orientation_delta_rpy[0] - T(b_(3)); full_residuals[4] = orientation_delta_rpy[1] - T(b_(4)); full_residuals[5] = orientation_delta_rpy[2] - T(b_(5)); @@ -158,4 +164,4 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( } // namespace fuse_constraints -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ \ No newline at end of file +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 7ec76458c..adc281aac 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -55,7 +55,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2angleaxis(b(3-6)^-1 * q)] || - * + * * In case the user is interested in implementing a cost function of the form: * * cost(X) = (X - mu)^T S^{-1} (X - mu) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 0579b30c7..f2d1b3c64 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -54,7 +54,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2eul(q) - b(3:5) ] || - * + * * Here, the matrix A can be of variable size, thereby permitting the computation of errors for * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in * implementing a cost function of the form: diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp index a5f2fdf9d..424515bed 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -94,7 +94,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint const fuse_variables::Orientation3DStamped & orientation2, const fuse_core::Vector6d & delta, const fuse_core::Matrix6d & covariance); - + /** * @brief Create a constraint using a measurement/prior of the relative 3D pose * @@ -107,7 +107,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) * @param[in] variable_indices The indices of the measured variables - */ + */ RelativePose3DStampedEulerConstraint( const std::string & source, const fuse_variables::Position3DStamped & position1, diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index bf4ad6519..68dd8a7a1 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -79,4 +79,4 @@ PLUGINLIB_EXPORT_CLASS( fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, - fuse_core::Constraint); \ No newline at end of file + fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index 56206e301..8afee0c88 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -53,7 +53,7 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( : fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT mean_(mean), sqrt_information_(covariance.inverse().llt().matrixU()) -{ +{ } AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( @@ -65,7 +65,7 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( const std::vector & variable_indices) : fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT mean_(partial_mean) -{ +{ // Compute the partial sqrt information matrix of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); @@ -76,19 +76,17 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); - for (size_t i = 0; i < variable_indices.size(); ++i) - { + for (size_t i = 0; i < variable_indices.size(); ++i) { sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); } } fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const { - if (sqrt_information_.rows() == 6) - { + if (sqrt_information_.rows() == 6) { return (sqrt_information_.transpose() * sqrt_information_).inverse(); } // Otherwise we need to compute the pseudoinverse @@ -122,14 +120,14 @@ void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const { return new NormalPriorPose3DEuler(sqrt_information_, mean_); - + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), - // sqrt_information_.rows()); - + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + // And including the followings: // #include // #include @@ -138,4 +136,6 @@ ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedEulerConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsolutePose3DStampedEulerConstraint, + fuse_core::Constraint); diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 1614cfa5d..741481ba1 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -192,7 +192,7 @@ fuse_core::Transaction marginalizeVariables( [&elimination_order, &marginalized_variables](const fuse_core::UUID & variable_uuid) { return elimination_order.exists(variable_uuid) && - elimination_order.at(variable_uuid) < marginalized_variables.size(); + elimination_order.at(variable_uuid) < marginalized_variables.size(); })); // NOLINT fuse_core::Transaction transaction; diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index 34269cedc..e8c44bab0 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -40,7 +40,9 @@ namespace fuse_constraints { -NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) +NormalPriorOrientation3D::NormalPriorOrientation3D( + const fuse_core::Matrix3d & A, + const fuse_core::Vector4d & b) : A_(A), b_(b) { @@ -61,7 +63,7 @@ bool NormalPriorOrientation3D::Evaluate( double observation_inverse[4] = { - b_(0), + b_(0), -b_(1), -b_(2), -b_(3) @@ -74,7 +76,7 @@ bool NormalPriorOrientation3D::Evaluate( // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis - + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 581e455dd..c3743a3f3 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -62,7 +62,7 @@ bool NormalPriorPose3D::Evaluate( double observation_inverse[4] = { - b_(3), + b_(3), -b_(4), -b_(5), -b_(6) @@ -78,11 +78,11 @@ bool NormalPriorPose3D::Evaluate( residuals[2] = parameters[0][2] - b_[2]; // Compute orientation residuals - // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp - // and use that to compute residuals and jacobians. + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis - + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index c4871aa3b..033083baa 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -42,7 +42,9 @@ namespace fuse_constraints { -NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) +NormalPriorPose3DEuler::NormalPriorPose3DEuler( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) : A_(A), b_(b) { @@ -74,7 +76,7 @@ bool NormalPriorPose3DEuler::Evaluate( // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map = A_ * full_residuals; if (jacobians != nullptr) { @@ -85,7 +87,7 @@ bool NormalPriorPose3DEuler::Evaluate( // Jacobian wrt orientation if (jacobians[1] != nullptr) { Eigen::Map> j_quat2angle_map(j_quat2rpy); - Eigen::Map(jacobians[1], num_residuals(), 4) = + Eigen::Map(jacobians[1], num_residuals(), 4) = A_.rightCols<3>() * j_quat2angle_map; } } diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp index 0c4e7eee3..a2bc7706a 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -83,11 +83,10 @@ RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); - for (size_t i = 0; i < variable_indices.size(); ++i) - { + for (size_t i = 0; i < variable_indices.size(); ++i) { sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); } } @@ -122,7 +121,8 @@ void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const { // TODO(giafranchini): implement cost function with analytical Jacobians - return new ceres::AutoDiffCostFunction( + return new ceres::AutoDiffCostFunction( new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), sqrt_information_.rows()); } @@ -130,4 +130,6 @@ ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedEulerConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::RelativePose3DStampedEulerConstraint, + fuse_core::Constraint); diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp index 5a80953fd..8be6b3f49 100755 --- a/fuse_constraints/src/variable_constraints.cpp +++ b/fuse_constraints/src/variable_constraints.cpp @@ -52,9 +52,9 @@ bool VariableConstraints::empty() const size_t VariableConstraints::size() const { auto sum_edges = [](const size_t input, const ConstraintCollection & edges) - { - return input + edges.size(); - }; + { + return input + edges.size(); + }; return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges); } diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 9d775cb5e..5b79011fc 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -151,7 +151,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = + Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); @@ -231,7 +231,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = + Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 2838692ac..2baef4473 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -129,7 +129,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, Covariance) 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') @@ -172,7 +173,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov, variable_indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') @@ -459,7 +460,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, + mean, cov); // Serialize the constraint into an archive diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index a477c3f6a..21a8fffa4 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -63,14 +63,15 @@ class NormalPriorPose3DTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d( + 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< qw, qx, qy, qz + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< qw, qx, qy, qz }; TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) @@ -84,9 +85,9 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) AutoDiffNormalPriorPose3D autodiff_cost_function( new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), num_residuals); - + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); -} \ No newline at end of file +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 29e4766eb..9a2dd8853 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -51,7 +51,8 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 3d cost functor using AutoDiffNormalPriorPose3DEuler = - ceres::AutoDiffCostFunction; /** @@ -63,35 +64,38 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d( + 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< roll, pitch, and yaw + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< roll, pitch, and yaw }; TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function auto rpy = Eigen::Vector3d::Random(); - full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); const fuse_constraints::NormalPriorPose3DEuler cost_function{full_sqrt_information, full_mean}; const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), num_residuals); - + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) +TEST_F( + NormalPriorPose3DEulerTestFixture, + AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) { // Create cost function for a subset of residuals // Version with y position = 0 @@ -118,7 +122,9 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) +TEST_F( + NormalPriorPose3DEulerTestFixture, + AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) { // Create cost function for a subset of residuals // Version with roll, pitch = 0 @@ -145,7 +151,9 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) +TEST_F( + NormalPriorPose3DEulerTestFixture, + AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) { // Create cost function for a subset of residuals // Version with z = 0, orientation = 0 @@ -170,7 +178,9 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); } -TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) +TEST_F( + NormalPriorPose3DEulerTestFixture, + AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) { // Create cost function for a subset of residuals // Version with position = 0, roll = 0 diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index fffe35642..9e701ecbb 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -207,16 +207,17 @@ TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; - expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT // Compare EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); - EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); } TEST(RelativePose3DStampedEulerConstraint, Optimization) @@ -477,7 +478,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) mean_delta, cov_delta, indices); - + // Create a relative pose constraint for 1m in the y direction std::vector indices_y {1, 2, 3, 4, 5}; fuse_core::Vector6d mean_delta_y; @@ -672,7 +673,8 @@ TEST(RelativePose3DStampedEulerConstraint, Serialization) 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, + orientation2, delta, cov); // Serialize the constraint into an archive diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 06fbd1209..ae2929598 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -109,7 +109,7 @@ template class CallbackWrapper : public CallbackWrapperBase { public: - using CallbackFunction = std::function; + using CallbackFunction = std::function; /** * @brief Constructor diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp index 6b7853f3c..597a5ba8e 100644 --- a/fuse_core/include/fuse_core/ceres_options.hpp +++ b/fuse_core/include/fuse_core/ceres_options.hpp @@ -54,10 +54,10 @@ * For a given Ceres Solver Option , the function ToString calls ceres::ToString */ #define CERES_OPTION_TO_STRING_DEFINITION(Option) \ - static inline const char * ToString(ceres::Option value) \ - { \ - return ceres::Option ## ToString(value); \ - } + static inline const char * ToString(ceres::Option value) \ + { \ + return ceres::Option ## ToString(value); \ + } /** * Defines FromString overloaded function to Ceres Options. @@ -65,10 +65,10 @@ * For a given Ceres Solver Option , the function FromString calls ceres::StringTo */ #define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ - static inline bool FromString(std::string string_value, ceres::Option * value) \ - { \ - return ceres::StringTo ## Option(string_value, value); \ - } + static inline bool FromString(std::string string_value, ceres::Option * value) \ + { \ + return ceres::StringTo ## Option(string_value, value); \ + } /** * Defines ToString and FromString overloaded functions for Ceres Options. @@ -76,8 +76,8 @@ * See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION. */ #define CERES_OPTION_STRING_DEFINITIONS(Option) \ - CERES_OPTION_TO_STRING_DEFINITION(Option) \ - CERES_OPTION_FROM_STRING_DEFINITION(Option) + CERES_OPTION_TO_STRING_DEFINITION(Option) \ + CERES_OPTION_FROM_STRING_DEFINITION(Option) #if !CERES_VERSION_AT_LEAST(2, 0, 0) /** @@ -99,8 +99,8 @@ static void UpperCase(std::string * input) inline const char * LoggingTypeToString(LoggingType type) { switch (type) { - CASESTR(SILENT); - CASESTR(PER_MINIMIZER_ITERATION); + CASESTR(SILENT); + CASESTR(PER_MINIMIZER_ITERATION); default: return "UNKNOWN"; } @@ -117,8 +117,8 @@ inline bool StringToLoggingType(std::string value, LoggingType * type) inline const char * DumpFormatTypeToString(DumpFormatType type) { switch (type) { - CASESTR(CONSOLE); - CASESTR(TEXTFILE); + CASESTR(CONSOLE); + CASESTR(TEXTFILE); default: return "UNKNOWN"; } diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 553e3dfb6..db4f4b4a8 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -67,10 +67,10 @@ * @endcode */ #define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ - fuse_core::Constraint::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Constraint::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -86,22 +86,22 @@ * @endcode */ #define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -119,17 +119,17 @@ * @endcode */ #define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -146,10 +146,10 @@ * @endcode */ #define FUSE_CONSTRAINT_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -166,10 +166,10 @@ * @endcode */ #define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp index 7205470ff..5fe3d760c 100644 --- a/fuse_core/include/fuse_core/eigen_gtest.hpp +++ b/fuse_core/include/fuse_core/eigen_gtest.hpp @@ -108,23 +108,23 @@ AssertionResult AssertMatrixNearHelper( // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ. // Don't use this in your code. #define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixEqualHelper( \ - #v1, \ - #v2, \ - v1, \ - v2), on_failure) + GTEST_ASSERT_( \ + ::testing::AssertMatrixEqualHelper( \ + #v1, \ + #v2, \ + v1, \ + v2), on_failure) // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR. // Don't use this in your code. #define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixNearHelper( \ - #v1, \ - #v2, \ - v1, \ - v2, \ - tol), on_failure) + GTEST_ASSERT_( \ + ::testing::AssertMatrixNearHelper( \ + #v1, \ + #v2, \ + v1, \ + v2, \ + tol), on_failure) // Define gtest macros for use with Eigen @@ -137,7 +137,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix */ #define EXPECT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) + GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for exact equality of two Eigen matrix-like objects. @@ -148,7 +148,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix */ #define ASSERT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) + GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) /** * @brief Non-fatal check for approximate equality of two Eigen matrix-like objects. @@ -160,7 +160,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ #define EXPECT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) + GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for approximate equality of two Eigen matrix-like objects. @@ -172,7 +172,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ #define ASSERT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) + GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) } // namespace testing diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp index a896a4c75..9cc026637 100644 --- a/fuse_core/include/fuse_core/fuse_macros.hpp +++ b/fuse_core/include/fuse_core/fuse_macros.hpp @@ -86,11 +86,11 @@ * Use in the public section of the class. */ #define FUSE_SMART_PTR_DEFINITIONS(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** * Defines smart pointer aliases and static functions for a class that contains fixed-sized @@ -104,15 +104,15 @@ */ #if __cpp_aligned_new #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) #else #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -124,55 +124,55 @@ * Use in the public section of the class. */ #define FUSE_SMART_PTR_ALIASES_ONLY(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) #define __FUSE_SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ - using ConstSharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; #define __FUSE_MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } #define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>( \ + Eigen::aligned_allocator<__VA_ARGS__>(), \ + std::forward(args) ...); \ + } #define __FUSE_WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ - using ConstWeakPtr = std::weak_ptr; + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; #define __FUSE_UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; + using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ + } #else #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } #endif #endif // FUSE_CORE__FUSE_MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index 4b9c96c0f..c0a918e3e 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -71,22 +71,22 @@ * @endcode */ #define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -104,17 +104,17 @@ * @endcode */ #define FUSE_GRAPH_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, and type() method @@ -130,9 +130,9 @@ * @endcode */ #define FUSE_GRAPH_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index 71812a5bf..bf0fb4eb5 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -59,10 +59,10 @@ * @endcode */ #define FUSE_LOSS_CLONE_DEFINITION(...) \ - fuse_core::Loss::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Loss::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -78,22 +78,22 @@ * @endcode */ #define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -111,17 +111,17 @@ * @endcode */ #define FUSE_LOSS_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -138,10 +138,10 @@ * @endcode */ #define FUSE_LOSS_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/include/fuse_core/macros.hpp b/fuse_core/include/fuse_core/macros.hpp index c78eb0966..3a46ed800 100644 --- a/fuse_core/include/fuse_core/macros.hpp +++ b/fuse_core/include/fuse_core/macros.hpp @@ -90,11 +90,11 @@ * Use in the public section of the class. */ #define SMART_PTR_DEFINITIONS(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** * Defines smart pointer aliases and static functions for a class that contains fixed-sized @@ -108,15 +108,15 @@ */ #if __cpp_aligned_new #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - SMART_PTR_DEFINITIONS(__VA_ARGS__) + SMART_PTR_DEFINITIONS(__VA_ARGS__) #else #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -128,55 +128,55 @@ * Use in the public section of the class. */ #define SMART_PTR_ALIASES_ONLY(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) #define __SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ - using ConstSharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; #define __MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } #define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ - } + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>( \ + Eigen::aligned_allocator<__VA_ARGS__>(), \ + std::forward(args) ...); \ + } #define __WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ - using ConstWeakPtr = std::weak_ptr; + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; #define __UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; + using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ + } #else #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } #endif #endif // FUSE_CORE__MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp index 408bbeea0..218f89ae0 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.hpp +++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp @@ -96,9 +96,9 @@ typename MessageBuffer::message_range MessageBuffer::query( // Find the entry that is strictly greater than the requested beginning stamp. If the extended // range flag is true, we will then back up one entry. auto upper_bound_comparison = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; + { + return element.first > stamp; + }; auto beginning_iter = std::upper_bound( buffer_.begin(), buffer_.end(), beginning_stamp, upper_bound_comparison); @@ -108,9 +108,9 @@ typename MessageBuffer::message_range MessageBuffer::query( // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is // false, we will back up one entry. auto lower_bound_comparison = [](const auto & element, const auto & stamp) -> bool - { - return element.first < stamp; - }; + { + return element.first < stamp; + }; auto ending_iter = std::lower_bound( buffer_.begin(), buffer_.end(), ending_stamp, lower_bound_comparison); @@ -155,9 +155,9 @@ void MessageBuffer::purgeHistory() // - at least two entries remains at all times // - the buffer covers *at least* until the expiration time. Longer is acceptable. auto is_greater = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; + { + return element.first > stamp; + }; auto expiration_iter = std::upper_bound( buffer_.begin(), buffer_.end(), expiration_time, is_greater); diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp index 8d38d644a..897e568ed 100644 --- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -31,16 +31,16 @@ #define ALL_FUSE_CORE_NODE_INTERFACES \ - fuse_core::node_interfaces::Base, \ - fuse_core::node_interfaces::Clock, \ - fuse_core::node_interfaces::Graph, \ - fuse_core::node_interfaces::Logging, \ - fuse_core::node_interfaces::Parameters, \ - fuse_core::node_interfaces::Services, \ - fuse_core::node_interfaces::TimeSource, \ - fuse_core::node_interfaces::Timers, \ - fuse_core::node_interfaces::Topics, \ - fuse_core::node_interfaces::Waitables + fuse_core::node_interfaces::Base, \ + fuse_core::node_interfaces::Clock, \ + fuse_core::node_interfaces::Graph, \ + fuse_core::node_interfaces::Logging, \ + fuse_core::node_interfaces::Parameters, \ + fuse_core::node_interfaces::Services, \ + fuse_core::node_interfaces::TimeSource, \ + fuse_core::node_interfaces::Timers, \ + fuse_core::node_interfaces::Topics, \ + fuse_core::node_interfaces::Waitables namespace fuse_core { diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 016b344b0..aa480ebac 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -62,10 +62,10 @@ * @endcode */ #define FUSE_VARIABLE_CLONE_DEFINITION(...) \ - fuse_core::Variable::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ - } + fuse_core::Variable::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ + } /** * @brief Implementation of the serialize() and deserialize() member functions for derived classes @@ -81,22 +81,22 @@ * @endcode */ #define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ - } + void serialize(fuse_core::BinaryOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive & archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive & archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive & archive) override \ + { \ + archive >> *this; \ + } /** * @brief Implements the type() member function using the suggested implementation @@ -114,17 +114,17 @@ * @endcode */ #define FUSE_VARIABLE_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ - } + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -141,10 +141,10 @@ * @endcode */ #define FUSE_VARIABLE_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) /** * @brief Convenience function that creates the required pointer aliases, clone() method, and type() @@ -161,10 +161,10 @@ * @endcode */ #define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) namespace fuse_core diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index d394b4be9..cbee6c839 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -92,9 +92,9 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite // Also don't add the same constraint twice auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; + { + return constraint_uuid == added_constraint->uuid(); + }; auto added_constraints_iter = std::find_if( added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -110,9 +110,9 @@ void Transaction::removeConstraint(const UUID & constraint_uuid) // If the constraint being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; + { + return constraint_uuid == added_constraint->uuid(); + }; auto added_constraints_iter = std::find_if( added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -166,9 +166,9 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) // Also don't add the same variable twice auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; + { + return variable_uuid == added_variable->uuid(); + }; auto added_variables_iter = std::find_if( added_variables_.begin(), added_variables_.end(), is_variable_added); @@ -184,9 +184,9 @@ void Transaction::removeVariable(const UUID & variable_uuid) // If the variable being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; + { + return variable_uuid == added_variable->uuid(); + }; auto added_variables_iter = std::find_if( added_variables_.begin(), added_variables_.end(), is_variable_added); diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 412c21696..8199d3c06 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -320,11 +320,11 @@ void HashGraph::getCovariance( // covariance matrix is symmetric, requesting Cov(A,B) and Cov(B,A) counts as a duplicate. Create // an expression to test a pair of data pointers such that (A,B) == (A,B) OR (B,A) auto symmetric_equal = [](const std::pair & x, - const std::pair & y) - { - return ((x.first == y.first) && (x.second == y.second)) || - ((x.first == y.second) && (x.second == y.first)); - }; + const std::pair & y) + { + return ((x.first == y.first) && (x.second == y.second)) || + ((x.first == y.second) && (x.second == y.first)); + }; // Convert the covariance requests into the input structure needed by Ceres. Namely, we must // convert the variable UUIDs into memory addresses. We create two containers of covariance // blocks: one only contains the unique variable pairs that we give to Ceres, and a second that diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index d9eeb4b5b..dcd51424f 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -148,7 +148,8 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value && !is_orientation::value, size_t> toIndex(const std::string & dimension) +std::enable_if_t::value && !is_orientation::value, size_t> toIndex( + const std::string & dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "roll" || lower_dim == "x") { @@ -167,7 +168,8 @@ std::enable_if_t::value && !is_orientation::value, size_t> t } template -std::enable_if_t::value && is_orientation::value, size_t> toIndex(const std::string & dimension) +std::enable_if_t::value && is_orientation::value, size_t> toIndex( + const std::string & dimension) { // Trick to get roll, pitch, yaw indexes as 0, 1, 2 auto lower_dim = boost::algorithm::to_lower_copy(dimension); diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 79886f829..8ece1ae8a 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -211,10 +211,10 @@ inline void populatePartialMeasurement( } /** - * @brief Method to create covariances of sub-measurements from covariances of full measurements and append + * @brief Method to create covariances of sub-measurements from covariances of full measurements and append * them to existing partial covariances * - * @param[in] covariance_full - The full covariance matrix from which we will generate the covariances of the + * @param[in] covariance_full - The full covariance matrix from which we will generate the covariances of the * sub-measurement * @param[in] indices - The indices we want to include in the sub-measurement * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append @@ -451,7 +451,7 @@ inline bool processAbsolutePoseWithCovariance( * fuse Transaction * * This method effectively adds two variables (3D position and 3D orientation) and a 3D pose - * constraint to the given \p transaction. The pose data is extracted from the \p pose message. + * constraint to the given \p transaction. The pose data is extracted from the \p pose message. * The data will be automatically transformed into the \p target_frame before it is used. * * @param[in] source - The name of the sensor or motion model that generated this constraint @@ -504,7 +504,8 @@ inline bool processAbsolutePose3DWithCovariance( position->x() = transformed_message.pose.pose.position.x; position->y() = transformed_message.pose.pose.position.y; position->z() = transformed_message.pose.pose.position.z; - auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + auto orientation = + fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); orientation->w() = transformed_message.pose.pose.orientation.w; orientation->x() = transformed_message.pose.pose.orientation.x; orientation->y() = transformed_message.pose.pose.orientation.y; @@ -522,7 +523,7 @@ inline bool processAbsolutePose3DWithCovariance( transformed_message.pose.pose.orientation.z; Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); - + if (validate) { try { validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); @@ -534,7 +535,7 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( source, *position, @@ -557,8 +558,11 @@ inline bool processAbsolutePose3DWithCovariance( tf2::fromMsg(transformed_message.pose.pose, tf2_pose); // Fill eigen pose in RPY representation fuse_core::Vector6d pose_mean_partial; - pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); - tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), + tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( + pose_mean_partial(3), pose_mean_partial( + 4), pose_mean_partial(5)); Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); @@ -567,12 +571,14 @@ inline bool processAbsolutePose3DWithCovariance( std::replace_if( pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), [&indices, &pose_mean_partial](const double & value) { - return std::find(indices.begin(), indices.end(), &value - pose_mean_partial.data()) == indices.end(); + return std::find( + indices.begin(), indices.end(), + &value - pose_mean_partial.data()) == indices.end(); }, 0.0); fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); populatePartialMeasurement( - pose_covariance, - indices, + pose_covariance, + indices, pose_covariance_partial); if (validate) { @@ -586,7 +592,7 @@ inline bool processAbsolutePose3DWithCovariance( return false; } } - + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( source, *position, @@ -594,7 +600,7 @@ inline bool processAbsolutePose3DWithCovariance( pose_mean_partial, pose_covariance_partial, indices); - + constraint->loss(loss); transaction.addVariable(position); @@ -1012,7 +1018,7 @@ inline bool processDifferentialPose3DWithCovariance( const std::vector & orientation_indices, const bool validate, fuse_core::Transaction & transaction) -{ +{ // Create the pose variables auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); auto orientation1 = @@ -1026,7 +1032,7 @@ inline bool processDifferentialPose3DWithCovariance( orientation1->w() = pose1.pose.pose.orientation.w; auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2.pose.pose.position.x; position2->y() = pose2.pose.pose.position.y; @@ -1043,29 +1049,29 @@ inline bool processDifferentialPose3DWithCovariance( // Covariance is Eigen::Matrix6d // N.B. covariance_geometry implements functions for pose composition and covariance propagation - // which are based on "A tutorial on SE(3) transformation parameterizations and on-manifold optimization" + // which are based on "A tutorial on SE(3) transformation parameterizations and on-manifold optimization" // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) - // Convert from ROS msg to covariance geometry types + // Convert from ROS msg to covariance geometry types covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; covariance_geometry::fromROS(pose1.pose, p1); covariance_geometry::fromROS(pose2.pose, p2); - // Create the delta for the constraint + // Create the delta for the constraint if (independent) { covariance_geometry::ComposePoseQuaternionCovarianceRPY( - covariance_geometry::InversePose3DQuaternionCovarianceRPY(p1), - p2, + covariance_geometry::InversePose3DQuaternionCovarianceRPY(p1), + p2, p12 ); } else { - // If covariances of p1 and p2 are the same, then it's possible that p12 covariance will be - // zero or ill-conditioned. To avoid this, we skip the expensive following calculations and + // If covariances of p1 and p2 are the same, then it's possible that p12 covariance will be + // zero or ill-conditioned. To avoid this, we skip the expensive following calculations and // instead we just add a minimum covariance later if (p1.second.isApprox(p2.second, 1e-9)) { covariance_geometry::ComposePose3DQuaternion( - covariance_geometry::InversePose(p1.first), - p2.first, + covariance_geometry::InversePose(p1.first), + p2.first, p12.first ); p12.second.setZero(); @@ -1073,7 +1079,7 @@ inline bool processDifferentialPose3DWithCovariance( // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. // We know cov1 and cov2 and we should substract the first to the second in order // to obtain the relative pose covariance. But first the 2 of them have to be in the - // same reference frame, moreover we need to rotate the result in p12 reference frame + // same reference frame, moreover we need to rotate the result in p12 reference frame // The covariance propagation of p2 = p1 * p12 is: // // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T @@ -1089,16 +1095,16 @@ inline bool processDifferentialPose3DWithCovariance( covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( p1, - p1_q + p1_q ); covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( p2, - p2_q + p2_q ); // Then we need to compute the delta pose covariance_geometry::ComposePose3DQuaternion( - covariance_geometry::InversePose(p1_q.first), - p2_q.first, + covariance_geometry::InversePose(p1_q.first), + p2_q.first, p12_q.first ); // Now we have to compute pose composition jacobians so we can rotate covariances @@ -1118,12 +1124,13 @@ inline bool processDifferentialPose3DWithCovariance( // TODO(giafranchini): check if faster to use j12.llt().solve() instead j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); - p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * j_p12_inv.transpose(); + p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * + j_p12_inv.transpose(); // Now again convert the delta pose covariance back to RPY(6x6) covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( - p12_q, - p12 + p12_q, + p12 ); } } @@ -1131,12 +1138,12 @@ inline bool processDifferentialPose3DWithCovariance( if (position_indices.size() == 3 && orientation_indices.size() == 3) { // Full pose measurement, no need to create a partial one fuse_core::Vector7d pose_relative_mean; - pose_relative_mean << - p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), - p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), + pose_relative_mean << + p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), + p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), p12.first.second.z(); fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; - + if (validate) { try { validateMeasurement( @@ -1175,9 +1182,13 @@ inline bool processDifferentialPose3DWithCovariance( // Convert the poses into RPY representation fuse_core::Vector6d pose_relative_mean_partial; - tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), p12.first.second.w()); - pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), p12.first.first.z(); - tf2::Matrix3x3(q12).getRPY(pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), + p12.first.second.w()); + pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), + p12.first.first.z(); + tf2::Matrix3x3(q12).getRPY( + pose_relative_mean_partial(3), pose_relative_mean_partial( + 4), pose_relative_mean_partial(5)); fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; @@ -1187,14 +1198,15 @@ inline bool processDifferentialPose3DWithCovariance( // Set the components which are not measured to zero std::replace_if( - pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + pose_relative_mean_partial.data(), + pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), [&indices, &pose_relative_mean_partial](const double & value) { return std::find( - indices.begin(), - indices.end(), + indices.begin(), + indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); }, 0.0); - + populatePartialMeasurement( pose_relative_covariance, indices, @@ -1537,8 +1549,9 @@ inline bool processDifferentialPose3DWithTwistCovariance( if (position_indices.size() == 3 && orientation_indices.size() == 3) { // Full pose measurement, no need to create a partial one fuse_core::Vector7d pose_relative_mean; - pose_relative_mean << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), - delta.getRotation().w(), delta.getRotation().x(), delta.getRotation().y(), delta.getRotation().z(); + pose_relative_mean << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), + delta.getRotation().w(), delta.getRotation().x(), delta.getRotation().y(), + delta.getRotation().z(); if (validate) { try { @@ -1579,21 +1592,25 @@ inline bool processDifferentialPose3DWithTwistCovariance( // Fill eigen pose in RPY representation fuse_core::Vector6d pose_relative_mean_partial; - pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(); + pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), + delta.getOrigin().z(); tf2::Matrix3x3(delta.getRotation()).getRPY( pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); - + // Set the components which are not measured to zero const auto indices = mergeIndices(position_indices, orientation_indices, 3); std::replace_if( - pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + pose_relative_mean_partial.data(), + pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), [&indices, &pose_relative_mean_partial](const double & value) { - return std::find(indices.begin(), indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); + return std::find( + indices.begin(), indices.end(), + &value - pose_relative_mean_partial.data()) == indices.end(); }, 0.0); fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); populatePartialMeasurement( - pose_relative_covariance, - indices, + pose_relative_covariance, + indices, pose_relative_covariance_partial); if (validate) { @@ -1876,13 +1893,13 @@ inline bool processTwist3DWithCovariance( // Create the mean twist vectors for the constraints fuse_core::Vector3d linear_vel_mean; linear_vel_mean << transformed_message.twist.twist.linear.x, - transformed_message.twist.twist.linear.y, + transformed_message.twist.twist.linear.y, transformed_message.twist.twist.linear.z; - + // Create the covariance for the constraint Eigen::Map linear_vel_covariance_map( transformed_message.twist.covariance.data()); - + // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); @@ -1933,12 +1950,13 @@ inline bool processTwist3DWithCovariance( velocity_angular->yaw() = transformed_message.twist.twist.angular.z; fuse_core::Vector3d angular_vel_mean; - angular_vel_mean << transformed_message.twist.twist.angular.x, - transformed_message.twist.twist.angular.y, + angular_vel_mean << transformed_message.twist.twist.angular.x, + transformed_message.twist.twist.angular.y, transformed_message.twist.twist.angular.z; // Create the covariance for the constraint - Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance.data()); + Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance. + data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); @@ -1951,7 +1969,7 @@ inline bool processTwist3DWithCovariance( angular_indices, angular_vel_mean_partial, angular_vel_covariance_partial); - + bool add_constraint = true; if (validate) { @@ -1969,7 +1987,8 @@ inline bool processTwist3DWithCovariance( if (add_constraint) { auto angular_vel_constraint = fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( - source, *velocity_angular, angular_vel_mean_partial, angular_vel_covariance_partial, angular_indices); + source, *velocity_angular, angular_vel_mean_partial, angular_vel_covariance_partial, + angular_indices); angular_vel_constraint->loss(angular_velocity_loss); @@ -2155,12 +2174,13 @@ inline bool processAccel3DWithCovariance( // Create the full mean vector and covariance for the constraint fuse_core::Vector3d accel_mean; - accel_mean << - transformed_message.accel.accel.linear.x, + accel_mean << + transformed_message.accel.accel.linear.x, transformed_message.accel.accel.linear.y, transformed_message.accel.accel.linear.z; - Eigen::Map> accel_covariance_map(transformed_message.accel.covariance.data()); + Eigen::Map> accel_covariance_map(transformed_message.accel.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); @@ -2253,7 +2273,7 @@ inline void scaleProcessNoiseCovariance( * @brief Scales the process noise covariance pose by the norm of the velocity * * @param[in, out] process_noise_covariance - The process noise covariance to scale. Only the pose - * components (x, y, z, roll, pitch, yaw) are scaled, and they are assumed to be in the + * components (x, y, z, roll, pitch, yaw) are scaled, and they are assumed to be in the * top left 6x6 corner * @param[in] velocity_linear - The linear velocity * @param[in] velocity_angular - The angular velocity @@ -2262,7 +2282,7 @@ inline void scaleProcessNoiseCovariance( */ inline void scaleProcessNoiseCovariance( fuse_core::Matrix15d & process_noise_covariance, - const fuse_core::Vector3d & velocity_linear, + const fuse_core::Vector3d & velocity_linear, const fuse_core::Vector3d & velocity_angular, const double velocity_linear_norm_min, const double velocity_angular_norm_min) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index f5081096f..5cec25309 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -65,8 +65,8 @@ namespace fuse_models * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal * values for the process noise covariance matrix. - * Variable order is (x, y, z, roll, pitch, yaw, - * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Variable order is (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc). */ class Omnidirectional3D : public fuse_core::AsyncMotionModel @@ -107,11 +107,11 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration //!< variable - fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) - fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities - fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations + fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations void print(std::ostream & stream = std::cout) const; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index bb20170cf..da92fb2d3 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -58,8 +58,8 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction + * omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this * ignition sensor resets the optimizer then publishes a new transaction with a prior at the @@ -73,12 +73,12 @@ namespace fuse_models * - ~initial_sigma (vector of doubles) A 15-dimensional vector containing the standard deviations * for the initial state values. The covariance matrix is * created placing the squared standard deviations along the - * diagonal of an 15x15 matrix. Variable order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * diagonal of an 15x15 matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for - * the state. Variable order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * the state. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before @@ -180,8 +180,8 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Create and send a prior transaction based on the supplied pose * - * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the + * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the * parameter server. * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 08a1b9007..f5a0030ac 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -93,7 +93,7 @@ inline void predict( const T acc_linear1_y, const T acc_linear1_z, const T dt, - T & position2_x, + T & position2_x, T & position2_y, T & position2_z, T & orientation2_r, @@ -120,23 +120,27 @@ inline void predict( const T dt2 = T(0.5) * dt * dt; // Project the state - position2_x = position1_x + - ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + - ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; position2_y = position1_y + - ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + - ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; - position2_z = position1_z + + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; + position2_z = position1_z + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + + + orientation2_r = orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -206,7 +210,7 @@ inline void predict( const double acc_linear1_y, const double acc_linear1_z, const double dt, - double & position2_x, + double & position2_x, double & position2_y, double & position2_z, double & orientation2_r, @@ -235,23 +239,27 @@ inline void predict( const double dt2 = 0.5 * dt * dt; // Project the state - position2_x = position1_x + - ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; position2_y = position1_y + - ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + - ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; + ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + + ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; position2_z = position1_z + ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + + + orientation2_r = orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -278,49 +286,62 @@ inline void predict( jacobian(1, 1) = 1.0; jacobian(2, 2) = 1.0; } - // Jacobian wrt orientation1 + // Jacobian wrt orientation1 if (jacobians[1]) { Eigen::Map> jacobian(jacobians[1]); Eigen::Map> j_quat2rpy_map(jacobian_quat2rpy); fuse_core::Matrix j_tmp; jacobian.setZero(); j_tmp.setZero(); - + // partial derivatives of position2_x wrt orientation1 - j_tmp(0, 0) = - ((cr * sp * cy + sr * sy) * vel_linear1_y + (- sr * sp * cy + cr * sy) * vel_linear1_z) * dt + - ((cr * sp * cy + sr * sy) * acc_linear1_y + (- sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; - j_tmp(0, 1) = - ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + - ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; - j_tmp(0, 2) = - ((-cp * sy) * vel_linear1_x + (- sr * sp * sy - cr * cy) * vel_linear1_y + (- cr * sp * sy + sr * cy) * vel_linear1_z) * dt + - ((-cp * sy) * acc_linear1_x + (- sr * sp * sy - cr * cy) * acc_linear1_y + (- cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; - - // partial derivatives of position2_y wrt orientation1 - j_tmp(1, 0) = - ((- sr * cy + cr * sp * sy) * vel_linear1_y + (- cr * cy - sr * sp * sy) * vel_linear1_z) * dt + - ((- sr * cy + cr * sp * sy) * acc_linear1_y + (- cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 1) = - ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + - ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 2) = - ((cp * cy) * vel_linear1_x + (- cr * sy + sr * sp * cy) * vel_linear1_y + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (- cr * sy + sr * sp * cy) * acc_linear1_y + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; - + j_tmp(0, 0) = + ((cr * sp * cy + sr * sy) * vel_linear1_y + (-sr * sp * cy + cr * sy) * vel_linear1_z) * + dt + + ((cr * sp * cy + sr * sy) * acc_linear1_y + (-sr * sp * cy + cr * sy) * acc_linear1_z) * + dt2; + j_tmp(0, 1) = + ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * + vel_linear1_z) * dt + + ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * + acc_linear1_z) * dt2; + j_tmp(0, 2) = + ((-cp * sy) * vel_linear1_x + (-sr * sp * sy - cr * cy) * vel_linear1_y + + (-cr * sp * sy + sr * cy) * vel_linear1_z) * dt + + ((-cp * sy) * acc_linear1_x + (-sr * sp * sy - cr * cy) * acc_linear1_y + + (-cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; + + // partial derivatives of position2_y wrt orientation1 + j_tmp(1, 0) = + ((-sr * cy + cr * sp * sy) * vel_linear1_y + (-cr * cy - sr * sp * sy) * vel_linear1_z) * + dt + + ((-sr * cy + cr * sp * sy) * acc_linear1_y + (-cr * cy - sr * sp * sy) * acc_linear1_z) * + dt2; + j_tmp(1, 1) = + ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * + vel_linear1_z) * dt + + ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * + acc_linear1_z) * dt2; + j_tmp(1, 2) = + ((cp * cy) * vel_linear1_x + (-cr * sy + sr * sp * cy) * vel_linear1_y + + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (-cr * sy + sr * sp * cy) * acc_linear1_y + + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; + // partial derivatives of position2_z wrt orientation1 - j_tmp(2, 0) = - ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + + j_tmp(2, 0) = + ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; - j_tmp(2, 1) = + j_tmp(2, 1) = (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; // partial derivatives of orientation2_r wrt orientation1 - j_tmp(3, 0) = 1.0 + + j_tmp(3, 0) = 1.0 + ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; - j_tmp(3, 1) = - ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr *sp * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp(3, 1) = + ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr * sp * sp * cpi * cpi) * + vel_angular1_y) * dt; // partial derivatives of orientation2_p wrt orientation1 j_tmp(4, 0) = (-sr * vel_angular1_p - cr * vel_angular1_y) * dt; @@ -328,11 +349,14 @@ inline void predict( // partial derivatives of orientation2_y wrt orientation1 j_tmp(5, 0) = ((cr * cpi) * vel_angular1_p - (sr * cpi) * vel_angular1_y) * dt; - j_tmp(5, 1) = ((sp * sr * cpi * cpi) * vel_angular1_p + (cr * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp( + 5, + 1) = ((sp * sr * cpi * cpi) * vel_angular1_p + (cr * sp * cpi * cpi) * vel_angular1_y) * + dt; j_tmp(5, 2) = 1.0; jacobian.block<6, 4>(0, 0) = j_tmp * j_quat2rpy_map; - } + } // Jacobian wrt vel_linear1 if (jacobians[2]) { Eigen::Map> jacobian(jacobians[2]); @@ -341,7 +365,7 @@ inline void predict( // partial derivatives of position1_x wrt vel_linear1 jacobian(0, 0) = cp * cy * dt; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; // partial derivatives of position1_y wrt vel_linear1 jacobian(1, 0) = cp * sy * dt; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt; @@ -388,7 +412,7 @@ inline void predict( // partial derivatives of position1_x wrt acc_linear1 jacobian(0, 0) = cp * cy * dt2; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt2; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; // partial derivatives of position1_y wrt acc_linear1 jacobian(1, 0) = cp * sy * dt2; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt2; @@ -542,8 +566,8 @@ inline void predict( // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); } /** @@ -584,7 +608,7 @@ inline void predict( // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per // parameter block and later reconstruct the fuse_core::Matrix15d with the full jacobian. The - // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, + // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, // vel_angular1: 3, acc_linear1: 3} static constexpr size_t num_residuals{15}; @@ -638,8 +662,8 @@ inline void predict( // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); } } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 49349a0e9..874042136 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -74,7 +74,7 @@ namespace fuse_models * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || * || [ z_acc_t2 - proj(z_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and * proj is a function that projects the state variables from time t1 to time t2. In case the user is * interested in implementing a cost function of the form @@ -84,7 +84,8 @@ namespace fuse_models * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the * square root information matrix (the inverse of the covariance). */ -class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> +class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, + 3, 3, 3> { public: FUSE_MAKE_ALIGNED_OPERATOR_NEW() @@ -94,7 +95,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); @@ -103,13 +104,13 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * @brief Evaluate the cost function. Used by the Ceres optimization engine. * * @param[in] parameters - Parameter blocks: - * 0 : position1 - First position (array with x at index 0, y at index 1, + * 0 : position1 - First position (array with x at index 0, y at index 1, * z at index 2) * 1 : orientation1 - First orientation (array with w at index 0, x at index 1, * y at index 2, z at index 3) * 2 : vel_linear1 - First linear velocity (array with x at index 0, y at * index 1, z at index 2) - * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y * at index 1, z at index 2) @@ -121,7 +122,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * index 1, z at index 2) * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) - * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at * index 1) * @param[out] residual - The computed residual (error) * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not @@ -179,7 +180,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 acc_linear_pred[0], acc_linear_pred[1], acc_linear_pred[2], - jacobians, + jacobians, j1_quat2rpy); residuals[0] = parameters[5][0] - position_pred[0]; @@ -237,7 +238,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } - + // Jacobian wrt position2 if (jacobians[5]) { Eigen::Map> jacobian(jacobians[5]); diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index 86a7827b4..4e8b28ea5 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -92,7 +92,7 @@ class Omnidirectional3DStateCostFunctor * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); @@ -128,7 +128,7 @@ class Omnidirectional3DStateCostFunctor private: double dt_; fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix + //!< information matrix }; Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor( @@ -183,7 +183,7 @@ bool Omnidirectional3DStateCostFunctor::operator()( vel_linear_pred, vel_angular_pred, acc_linear_pred); - + Eigen::Map> residuals_map(residual); residuals_map(0) = position2[0] - position_pred[0]; residuals_map(1) = position2[1] - position_pred[1]; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index f196c55bc..7201cdbed 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -91,7 +91,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @param[in] velocity_angular2 Angular velocity component variable of the second state * @param[in] acceleration_linear2 Linear acceleration component variable of the second state * @param[in] covariance The covariance matrix used to weight the constraint. Order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ Omnidirectional3DStateKinematicConstraint( @@ -122,7 +122,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ const fuse_core::Matrix15d & sqrtInformation() const {return sqrt_information_;} @@ -130,7 +130,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint /** * @brief Compute the measurement covariance matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ fuse_core::Matrix15d covariance() const diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 1c0dda5c5..4051799d0 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -106,12 +106,12 @@ struct Imu3DParams : public ParameterBase fuse_core::getPositiveParam( interfaces, fuse_core::joinParameterName( ns, - "tf_timeout"), tf_timeout, + "tf_timeout"), tf_timeout, false); fuse_core::getPositiveParam( interfaces, fuse_core::joinParameterName( ns, - "throttle_period"), throttle_period, + "throttle_period"), throttle_period, false); throttle_use_wall_time = fuse_core::getParam( diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 1e01a073c..ed9059fa6 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -73,7 +73,7 @@ struct Odometry3DParams : public ParameterBase > interfaces, const std::string & ns) { - position_indices = + position_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName( ns, diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index acb1fd286..c16658d67 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -193,18 +193,18 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase * The covariance matrix is created placing the squared standard deviations along the diagonal of * an 15x15 matrix. */ - std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 - }; + std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 + }; /** - * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) */ std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0}; /** * @brief Loss function */ diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp index 52f9a9302..8a3ca5d4f 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp @@ -115,9 +115,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase std::to_string(sigma_vector.size())); } auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; + { + return std::isfinite(sigma) && (sigma > 0); + }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { throw std::invalid_argument( "The supplied initial_sigma parameter must contain valid floating point values. " @@ -139,9 +139,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase std::to_string(state_vector.size())); } auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; + { + return std::isfinite(state); + }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { throw std::invalid_argument( "The supplied initial_state parameter must contain valid floating point values. " diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 9fdc11d4f..d01bf0997 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -167,7 +167,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - + const bool validate = !params_.disable_checks; if (params_.differential) { @@ -248,7 +248,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) void Imu3D::processDifferential( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, fuse_core::Transaction & transaction) { @@ -267,9 +267,9 @@ void Imu3D::processDifferential( return; } - if (!previous_pose_) { + if (!previous_pose_) { previous_pose_ = std::move(transformed_pose); - return; + return; } if (params_.use_twist_covariance) { diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 85cb1cc7f..4afd0ee39 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -107,23 +107,23 @@ void Odometry3D::onStart() if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) { - previous_pose_.reset(); + previous_pose_.reset(); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = cb_group_; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &OdometryThrottledCallback::callback< - const nav_msgs::msg::Odometry &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &OdometryThrottledCallback::callback< + const nav_msgs::msg::Odometry &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 6fb94e5ee..f1e00267a 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -198,7 +198,9 @@ void Odometry3DPublisher::notifyCallback( covariance_requests.emplace_back(acceleration_linear_uuid, acceleration_linear_uuid); std::vector> covariance_matrices; - graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options, true); + graph->getCovariance( + covariance_requests, covariance_matrices, params_.covariance_options, + true); odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) @@ -206,7 +208,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) - + odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) @@ -262,7 +264,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[15] = covariance_matrices[4][6]; odom_output.twist.covariance[16] = covariance_matrices[4][7]; odom_output.twist.covariance[17] = covariance_matrices[4][8]; - + odom_output.twist.covariance[18] = covariance_matrices[4][0]; odom_output.twist.covariance[19] = covariance_matrices[4][3]; odom_output.twist.covariance[20] = covariance_matrices[4][6]; @@ -283,7 +285,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[33] = covariance_matrices[5][2]; odom_output.twist.covariance[34] = covariance_matrices[5][5]; odom_output.twist.covariance[35] = covariance_matrices[5][8]; - + acceleration_output.accel.covariance[0] = covariance_matrices[6][0]; acceleration_output.accel.covariance[1] = covariance_matrices[6][1]; acceleration_output.accel.covariance[2] = covariance_matrices[6][2]; @@ -407,7 +409,7 @@ bool Odometry3DPublisher::getState( auto acceleration_linear_variable = dynamic_cast( graph.getVariable(acceleration_linear_uuid)); - + odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = position_variable.z(); @@ -482,9 +484,9 @@ void Odometry3DPublisher::publishTimerCallback() orientation.y() = pose.getRotation().y(); orientation.z() = pose.getRotation().z(); orientation.w() = pose.getRotation().w(); - velocity_linear << odom_output.twist.twist.linear.x, + velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y, odom_output.twist.twist.linear.z; - velocity_angular << odom_output.twist.twist.angular.x, + velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y, odom_output.twist.twist.angular.z; const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); @@ -493,7 +495,7 @@ void Odometry3DPublisher::publishTimerCallback() fuse_core::Vector3d acceleration_linear; if (params_.predict_with_acceleration) { - acceleration_linear << acceleration_output.accel.accel.linear.x, + acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; } @@ -513,7 +515,10 @@ void Odometry3DPublisher::publishTimerCallback() // Convert back to tf2 representation pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); - pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w())); + pose.setRotation( + tf2::Quaternion( + orientation.x(), orientation.y(), orientation.z(), + orientation.w())); odom_output.pose.pose.position.x = position.x(); odom_output.pose.pose.position.y = position.y(); @@ -546,7 +551,8 @@ void Odometry3DPublisher::publishTimerCallback() covariance.setZero(); Eigen::Map pose_covariance(odom_output.pose.covariance.data()); Eigen::Map twist_covariance(odom_output.twist.covariance.data()); - Eigen::Map acceleration_covariance(acceleration_output.accel.covariance.data()); + Eigen::Map acceleration_covariance(acceleration_output.accel.covariance. + data()); covariance.block<6, 6>(0, 0) = pose_covariance; covariance.block<6, 6>(6, 6) = twist_covariance; @@ -557,9 +563,9 @@ void Odometry3DPublisher::publishTimerCallback() auto process_noise_covariance = params_.process_noise_covariance; if (params_.scale_process_noise) { common::scaleProcessNoiseCovariance( - process_noise_covariance, + process_noise_covariance, velocity_linear, - velocity_angular, + velocity_angular, params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_); } @@ -588,7 +594,7 @@ void Odometry3DPublisher::publishTimerCallback() trans.header.stamp = odom_output.header.stamp; trans.header.frame_id = frame_id; trans.child_frame_id = child_frame_id; - trans.transform = tf2::toMsg(pose); + trans.transform = tf2::toMsg(pose); if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { try { auto base_to_odom = tf_buffer_->lookupTransform( diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index 92534d25d..206199df6 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -174,7 +174,7 @@ bool Omnidirectional3D::applyCallback(fuse_core::Transaction & transaction) // manager, in turn, makes calls to the generateMotionModel() function. try { // Now actually generate the motion model segments - timestamp_manager_.query(transaction, true); + timestamp_manager_.query(transaction, true); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM_THROTTLE( logger_, *clock_, 10.0 * 1000, @@ -228,7 +228,7 @@ void Omnidirectional3D::onInit() name_, "velocity_linear_norm_min"), velocity_linear_norm_min_); - + velocity_angular_norm_min_ = fuse_core::getParam( interfaces_, fuse_core::joinParameterName( @@ -317,13 +317,14 @@ void Omnidirectional3D::generateMotionModel( } else { state1 = base_state; } - + // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); if (dt == 0.0) { state1.position_uuid = fuse_variables::Position3DStamped(beginning_stamp, device_id_).uuid(); - state1.orientation_uuid = fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); + state1.orientation_uuid = + fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); state1.vel_linear_uuid = fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); state1.vel_angular_uuid = @@ -349,9 +350,10 @@ void Omnidirectional3D::generateMotionModel( state2.vel_angular, state2.acc_linear); - // Define the fuse variables required for this constraint + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); - auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( beginning_stamp, device_id_); @@ -380,14 +382,16 @@ void Omnidirectional3D::generateMotionModel( orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.orientation.w(); - + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.vel_linear.x(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.vel_linear.y(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.vel_linear.z(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.vel_angular.y(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = + state1.vel_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = + state1.vel_angular.y(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = state1.acc_linear.x(); @@ -395,7 +399,7 @@ void Omnidirectional3D::generateMotionModel( state1.acc_linear.y(); acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state1.acc_linear.z(); - + position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); @@ -404,14 +408,16 @@ void Omnidirectional3D::generateMotionModel( orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.orientation.w(); - + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.vel_linear.x(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.vel_linear.y(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.vel_linear.z(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.vel_angular.y(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = + state2.vel_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = + state2.vel_angular.y(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = state2.acc_linear.x(); @@ -525,7 +531,7 @@ void Omnidirectional3D::updateStateHistoryEstimates( { const auto & current_stamp = current_iter->first; auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.orientation_uuid) && graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_angular_uuid) && @@ -553,8 +559,8 @@ void Omnidirectional3D::updateStateHistoryEstimates( vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; current_state.vel_linear.z() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; - - current_state.vel_angular.x() = + + current_state.vel_angular.x() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; current_state.vel_angular.y() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; @@ -571,7 +577,7 @@ void Omnidirectional3D::updateStateHistoryEstimates( auto previous_iter = std::prev(current_iter); const auto & previous_stamp = previous_iter->first; const auto & previous_state = previous_iter->second; - + // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index 519f45e1d..422485418 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -236,7 +236,9 @@ void Omnidirectional3DIgnition::process( throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || !std::isfinite(pose.pose.pose.position.z)) { + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || + !std::isfinite(pose.pose.pose.position.z)) + { throw std::invalid_argument( "Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + @@ -263,8 +265,8 @@ void Omnidirectional3DIgnition::process( // } // } position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; if (!fuse_core::isSymmetric(position_cov)) { throw std::invalid_argument( "Attempting to set the pose with a non-symmetric position covariance matrix\n " + @@ -282,8 +284,8 @@ void Omnidirectional3DIgnition::process( // } // } orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; if (!fuse_core::isSymmetric(orientation_cov)) { throw std::invalid_argument( "Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + @@ -292,7 +294,9 @@ void Omnidirectional3DIgnition::process( if (!fuse_core::isPositiveDefinite(orientation_cov)) { throw std::invalid_argument( "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + - fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + fuse_core::to_string( + orientation_cov, + Eigen::FullPrecision) + "."); } // Tell the optimizer to reset before providing the initial state if (!params_.reset_service.empty()) { @@ -328,7 +332,8 @@ void Omnidirectional3DIgnition::process( } } -void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Omnidirectional3DIgnition::sendPrior( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { const auto & stamp = pose.header.stamp; @@ -352,7 +357,9 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova angular_velocity->roll() = params_.initial_state[9]; angular_velocity->pitch() = params_.initial_state[10]; angular_velocity->yaw() = params_.initial_state[11]; - auto linear_acceleration = fuse_variables::AccelerationLinear3DStamped::make_shared(stamp, device_id_); + auto linear_acceleration = fuse_variables::AccelerationLinear3DStamped::make_shared( + stamp, + device_id_); linear_acceleration->x() = params_.initial_state[12]; linear_acceleration->y() = params_.initial_state[13]; linear_acceleration->z() = params_.initial_state[14]; @@ -379,24 +386,24 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova // } position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; - + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, - 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; - + 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, + 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; + angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, - 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, - 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; - + 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, + 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; + linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, - 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, - 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; + 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, + 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; // Create absolute constraints for each variable auto position_constraint = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( name(), @@ -419,13 +426,17 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( name(), *angular_velocity, - fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), + fuse_core::Vector3d( + angular_velocity->roll(), angular_velocity->pitch(), + angular_velocity->yaw()), angular_velocity_cov); auto linear_acceleration_constraint = fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( name(), *linear_acceleration, - fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), + fuse_core::Vector3d( + linear_acceleration->x(), linear_acceleration->y(), + linear_acceleration->z()), linear_acceleration_cov); // Create the transaction @@ -450,7 +461,7 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova logger_, "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() << ", x: " << position->x() << ", y: " - << position->y() << ", z: " << position->z() + << position->y() << ", z: " << position->z() << ", roll: " << orientation->roll() << ", pitch: " << orientation->pitch() << ", yaw: " << orientation->yaw() << ")"); diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 7caf9dc3b..b92b7f031 100644 --- a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -114,4 +114,6 @@ ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() } // namespace fuse_models BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Omnidirectional3DStateKinematicConstraint); -PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3DStateKinematicConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_models::Omnidirectional3DStateKinematicConstraint, + fuse_core::Constraint); diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp index f82bb75a2..c525189af 100644 --- a/fuse_models/test/test_omnidirectional_3d.cpp +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -32,7 +32,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto position1 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0)); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); - auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); + auto angular_velocity1 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_acceleration1 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; @@ -54,7 +55,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto position2 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(2, 0)); auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); - auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); + auto angular_velocity2 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_acceleration2 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; @@ -76,7 +78,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto position3 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(3, 0)); auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); - auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); + auto angular_velocity3 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_acceleration3 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; @@ -98,7 +101,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto position4 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(4, 0)); auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); - auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); + auto angular_velocity4 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_acceleration4 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; @@ -120,7 +124,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto position5 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(5, 0)); auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); - auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); + auto angular_velocity5 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_acceleration5 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; @@ -295,8 +300,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The third entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(3, 0)].position; @@ -309,7 +314,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); - auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); @@ -330,8 +335,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The forth entry is included in the graph. It will get updated directly. auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(4, 0)].position; @@ -365,8 +370,8 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The fifth entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(5, 0)].position; @@ -389,7 +394,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); - EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; diff --git a/fuse_models/test/test_omnidirectional_3d_ignition.cpp b/fuse_models/test/test_omnidirectional_3d_ignition.cpp index 06ac28812..ece200d38 100644 --- a/fuse_models/test/test_omnidirectional_3d_ignition.cpp +++ b/fuse_models/test/test_omnidirectional_3d_ignition.cpp @@ -169,7 +169,7 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, InitialTransaction) orientation_actual.w() = actual->mean()[0]; orientation_actual.x() = actual->mean()[1]; orientation_actual.y() = actual->mean()[2]; - orientation_actual.z() = actual->mean()[3]; + orientation_actual.z() = actual->mean()[3]; EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-9); EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-9); EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-9); @@ -262,18 +262,18 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); - Eigen::Quaterniond q; + Eigen::Quaterniond q; q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad - + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + // Call the SetPose service auto srv = std::make_shared(); srv->pose.header.stamp = rclcpp::Time(12, 345678910); srv->pose.pose.pose.position.x = 1.0; srv->pose.pose.pose.position.y = 2.0; srv->pose.pose.pose.position.z = 3.0; - srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.x = q.x(); srv->pose.pose.pose.orientation.y = q.y(); srv->pose.pose.pose.orientation.z = q.z(); srv->pose.pose.pose.orientation.w = q.w(); @@ -283,7 +283,8 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) srv->pose.pose.covariance[21] = 4.0; srv->pose.pose.covariance[28] = 5.0; srv->pose.pose.covariance[35] = 6.0; - auto client = node->create_client("/omnidirectional_3d_ignition_test/set_pose"); + auto client = node->create_client( + "/omnidirectional_3d_ignition_test/set_pose"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); @@ -317,7 +318,7 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) orientation_actual.w() = actual->mean()[0]; orientation_actual.x() = actual->mean()[1]; orientation_actual.y() = actual->mean()[2]; - orientation_actual.z() = actual->mean()[3]; + orientation_actual.z() = actual->mean()[3]; EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); @@ -380,10 +381,10 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); - Eigen::Quaterniond q; + Eigen::Quaterniond q; q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad // Call the SetPose service auto srv = std::make_shared(); @@ -414,7 +415,7 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) // Check the transaction auto transaction = callback_future.get(); - { + { fuse_core::Vector3d expected_mean; expected_mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d expected_cov; @@ -435,7 +436,7 @@ TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) orientation_actual.w() = actual->mean()[0]; orientation_actual.x() = actual->mean()[1]; orientation_actual.y() = actual->mean()[2]; - orientation_actual.z() = actual->mean()[3]; + orientation_actual.z() = actual->mean()[3]; EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); // not high precision EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp index 230c0b8cc..63648087a 100644 --- a/fuse_models/test/test_omnidirectional_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -43,11 +43,11 @@ TEST(Predict, predictDirectVals) { - fuse_core::Vector3d position1 (0.0, 0.0, 0.0); - Eigen::Quaterniond orientation1 (1.0, 0.0, 0.0, 0.0); - fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); - fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); - fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d position1(0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1(1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1(1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1(1.0, 0.0, 0.0); double dt = 0.1; fuse_core::Vector3d position2; Eigen::Quaterniond orientation2; @@ -70,8 +70,8 @@ TEST(Predict, predictDirectVals) Eigen::Quaterniond q; q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); EXPECT_DOUBLE_EQ(0.0, position2.y()); @@ -105,9 +105,9 @@ TEST(Predict, predictDirectVals) acc_linear2); q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); EXPECT_DOUBLE_EQ(0.0, position2.z()); @@ -142,10 +142,10 @@ TEST(Predict, predictDirectVals) vel_linear2, vel_angular2, acc_linear2); - + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); EXPECT_DOUBLE_EQ(-0.105, position2.y()); @@ -268,16 +268,16 @@ TEST(Predict, predictFromDoublePointers) acc_linear2); EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(0.0, position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); @@ -285,29 +285,29 @@ TEST(Predict, predictFromDoublePointers) // Carry on with the output state from last time - show in-place update support fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); - EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); @@ -319,34 +319,34 @@ TEST(Predict, predictFromDoublePointers) acc_linear1[1] = -1.0; fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(-0.105,position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(-0.105, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); - EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - + // Out of plane motion position1[0] = 0.0; position1[1] = 0.0; @@ -428,11 +428,11 @@ TEST(Predict, predictFromDoublePointers) ); EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); - EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); - EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); - EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); - EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); @@ -474,16 +474,16 @@ TEST(Predict, predictFromJetPointers) acc_linear2); EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); @@ -502,27 +502,27 @@ TEST(Predict, predictFromJetPointers) vel_linear2, vel_angular2, acc_linear2); - - EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // // Use non-zero Y values - vel_linear1[1] = Jet(-1.0); + vel_linear1[1] = Jet(-1.0); vel_angular1[2] = Jet(-1.570796327); - acc_linear1[1] = Jet(-1.0); + acc_linear1[1] = Jet(-1.0); fuse_models::predict( position1, @@ -536,22 +536,22 @@ TEST(Predict, predictFromJetPointers) vel_linear2, vel_angular2, acc_linear2); - + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // Out of plane motion position1[0] = Jet(0.0); @@ -634,11 +634,11 @@ TEST(Predict, predictFromJetPointers) ); EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); - EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); - EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); @@ -648,4 +648,4 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); -} \ No newline at end of file +} diff --git a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp index bea153f97..a5bc835f8 100644 --- a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -47,9 +47,9 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); const double dt{0.1}; @@ -66,8 +66,8 @@ TEST(CostFunction, evaluateCostFunction) const double position2[3] = {0.105, 0.105, 0.105}; Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); const double orientation2[4] = {q2.w(), q2.x(), q2.y(), q2.z()}; const double vel_linear2[3] = {1.1, 1.1, 1.1}; const double vel_angular2[3] = {1.570796327, 1.570796327, 1.570796327}; @@ -120,8 +120,11 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction cost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor( + dt, + sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); std::vector jacobians_autodiff(num_parameter_blocks); diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp index c71d24bf6..d45d3b987 100644 --- a/fuse_models/test/test_sensor_proc_3d.cpp +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -100,18 +100,21 @@ TEST(TestSuite, populatePartialMeasurements) // Test both conversion from quaternion to RPY and partial measurement population // This one is just to generate a random unit quaternion and have the reference in RPY fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); - Eigen::Quaterniond q = + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); - + tf2::Transform tf2_pose; tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); fuse_core::Vector6d pose_mean_partial; - pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); - tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); - + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), + tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( + pose_mean_partial(3), pose_mean_partial( + 4), pose_mean_partial(5)); + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); const std::vector position_indices{0, 1}; @@ -122,13 +125,13 @@ TEST(TestSuite, populatePartialMeasurements) const auto merged_indices = fm_common::mergeIndices( position_indices, orientation_indices, orientation_offset); - + std::replace_if( pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), [&merged_indices, &pose_mean_partial](const double & value) { return std::find( - merged_indices.begin(), - merged_indices.end(), + merged_indices.begin(), + merged_indices.end(), &value - pose_mean_partial.data()) == merged_indices.end(); }, 0.0); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 096928a2a..4fbcf43f9 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -185,13 +185,13 @@ void FixedLagSmoother::postprocessMarginalization( void FixedLagSmoother::optimizationLoop() { auto exit_wait_condition = [this]() - { - return - this->optimization_request_ || - !this->optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid() - ; - }; + { + return + this->optimization_request_ || + !this->optimization_running_ || + !interfaces_.get_node_base_interface()->get_context()->is_valid() + ; + }; // Optimize constraints until told to exit while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) @@ -518,9 +518,9 @@ void FixedLagSmoother::transactionCallback( // The pending set is arranged "smallest stamp last" to making popping off the back more // efficient auto comparator = [](const rclcpp::Time & value, const TransactionQueueElement & element) - { - return value >= element.stamp(); - }; + { + return value >= element.stamp(); + }; auto position = std::upper_bound( pending_transactions_.begin(), pending_transactions_.end(), @@ -550,7 +550,7 @@ void FixedLagSmoother::transactionCallback( [&sensor_name, max_time, & min_time = start_time](const auto & transaction) { // NOLINT(whitespace/braces) return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); }), // NOLINT(whitespace/braces) pending_transactions_.end()); } else { diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 16c3cfa09..e23bd3353 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -502,9 +502,9 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists"); auto print_key = [](const std::string & result, const auto & entry) - { - return result + entry.first + ' '; - }; + { + return result + entry.first + ' '; + }; status.add( "Sensor Models", diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index 2a5b71775..60038053d 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -47,9 +47,9 @@ namespace fuse_optimizers rclcpp::Time VariableStampIndex::currentStamp() const { auto compare_stamps = [](const StampedMap::value_type & lhs, const StampedMap::value_type & rhs) - { - return lhs.second < rhs.second; - }; + { + return lhs.second < rhs.second; + }; auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); if (iter != stamped_index_.end()) { return iter->second; diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 7bd933208..b4a899117 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -86,7 +86,7 @@ struct Robot /** * @brief Convert the robot state into a ground truth odometry message */ -nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) { auto msg = std::make_shared(); msg->header.stamp = state.stamp; @@ -128,7 +128,9 @@ nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) /** * @brief Compute the next robot state given the current robot state and a simulated step time */ -Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, Eigen::Vector3d external_force) +Robot simulateRobotMotion( + const Robot & previous_state, const rclcpp::Time & now, + Eigen::Vector3d external_force) { auto dt = (now - previous_state.stamp).seconds(); auto next_state = Robot(); @@ -162,11 +164,11 @@ Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, /** * @brief Create a simulated Imu measurement from the current state */ -sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) { static std::random_device rd{}; - static std::mt19937 generator{ rd() }; - static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + static std::mt19937 generator{rd()}; + static std::normal_distribution<> noise{0.0, IMU_SIGMA}; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -194,11 +196,11 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) return msg; } -nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot& robot) +nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot & robot) { static std::random_device rd{}; - static std::mt19937 generator{ rd() }; - static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; + static std::mt19937 generator{rd()}; + static std::normal_distribution<> position_noise{0.0, ODOM_POSITION_SIGMA}; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -226,8 +228,9 @@ nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot& robot) return msg; } -void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, - const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) +void initializeStateEstimation( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot & state, const rclcpp::Clock::SharedPtr & clock, const rclcpp::Logger & logger) { // Send the initial localization signal to the state estimator auto srv = std::make_shared(); @@ -249,24 +252,27 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacespose.pose.covariance[35] = 1.0; auto client = rclcpp::create_client( - interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), - interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", + rclcpp::ServicesQoS()); while (!client->wait_for_service(std::chrono::seconds(30)) && - interfaces.get_node_base_interface()->get_context()->is_valid()) + interfaces.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + RCLCPP_WARN_STREAM( + logger, + "Waiting for '" << client->get_service_name() << "' service to become available."); } auto success = false; - while (!success) - { + while (!success) { clock->sleep_for(std::chrono::milliseconds(100)); srv->pose.header.stamp = clock->now(); auto result_future = client->async_send_request(srv); - if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future, - std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete( + interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(logger, "service call failed :("); client->remove_pending_request(result_future); @@ -276,7 +282,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacesnow(); auto const now = node->now(); @@ -318,23 +323,16 @@ int main(int argc, char** argv) // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) double const force_magnitude = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); - Eigen::Vector3d external_force = { 0, 0, 0 }; + Eigen::Vector3d external_force = {0, 0, 0}; // switch oscillation axes every `motion_duration` seconds (with one 'rest period') - if (std::fmod(now_d, 4 * motion_duration) < motion_duration) - { + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) { external_force.x() = force_magnitude; - } - else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) - { + } else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) { external_force.y() = force_magnitude; - } - else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) - { + } else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) { external_force.z() = force_magnitude; - } - else - { + } else { // reset the robot's position and velocity, leave the external force as 0 // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) state.x = 0; From 43c58338944c1ad7ffac9d137f37a14f98ff4830 Mon Sep 17 00:00:00 2001 From: giacomo Date: Tue, 22 Oct 2024 17:16:02 +0200 Subject: [PATCH 113/116] test quaternionToAngleAxis for very small rotations --- fuse_core/include/fuse_core/util.hpp | 163 ++++++++++++++++----------- fuse_core/test/test_util.cpp | 126 ++++++++++++++------- 2 files changed, 183 insertions(+), 106 deletions(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index c404fc767..1b10f088c 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -158,7 +159,7 @@ Eigen::Matrix rotationMatrix2D(const T angle) * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) */ -static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr) +static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr) { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); @@ -170,8 +171,8 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob const double qx = q[1]; const double qy = q[2]; const double qz = q[3]; - const double discr = qw * qy - qx * qz; - const double gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); + const double discr = qw * qy - qx * qz; + const double gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); if (discr > gl_limit) { // pitch = 90 deg @@ -189,82 +190,114 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob // Non-degenerate case: jacobian_map(0, 0) = -(2.0 * qx) / - ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + ((std::pow( + (2.0 * qw * qx + 2.0 * qy * qz), + 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); jacobian_map(0, 1) = -((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - - (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / - (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / + std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow( + (2.0 * qw * qx + 2.0 * qy * qz), + 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); jacobian_map(0, 2) = -((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - - (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / - (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / + std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow( + (2.0 * qw * qx + 2.0 * qy * qz), + 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); jacobian_map(0, 3) = -(2.0 * qy) / - ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + ((std::pow( + (2.0 * qw * qx + 2.0 * qy * qz), + 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); - jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map( + 1, + 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map( + 1, + 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map( + 1, + 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map( + 1, + 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); jacobian_map(2, 0) = -(2.0 * qz) / - ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + ((std::pow( + (2.0 * qw * qz + 2.0 * qx * qy), + 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) * (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); jacobian_map(2, 1) = -(2.0 * qy) / - ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + ((std::pow( + (2.0 * qw * qz + 2.0 * qx * qy), + 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) * (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); jacobian_map(2, 2) = -((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - - (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / - (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / + std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow( + (2.0 * qw * qz + 2.0 * qx * qy), + 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); jacobian_map(2, 3) = -((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - - (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / - (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / + std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow( + (2.0 * qw * qz + 2.0 * qx * qy), + 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); } } } /** * @brief Compute product of two quaternions and the function jacobian - * TODO(giafranchini): parametric jacobian computation? Atm this function is only used in + * TODO(giafranchini): parametric jacobian computation? Atm this function is only used in * normal_prior_pose_3d cost function. There we only need the derivatives wrt quaternion W, * so at the time we are only computing the jacobian wrt W - * + * * @param[in] z Pointer to the first quaternion array (4x1) * @param[in] w Pointer to the second quaternion array (4x1) * @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w * @param[in] jacobian Pointer to the jacobian of zw with respect to w (4x4, optional) */ -static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr) +static inline void quaternionProduct( + const double * z, const double * w, double * zw, + double * jacobian = nullptr) { ceres::QuaternionProduct(z, w, zw); if (jacobian) { Eigen::Map> jacobian_map(jacobian); - jacobian_map << + jacobian_map << z[0], -z[1], -z[2], -z[3], - z[1], z[0], -z[3], z[2], - z[2], z[3], z[0], -z[1], - z[3], -z[2], z[1], z[0]; + z[1], z[0], -z[3], z[2], + z[2], z[3], z[0], -z[1], + z[3], -z[2], z[1], z[0]; } } /** * @brief Compute quaternion to AngleAxis conversion and the function jacobian - * + * * @param[in] q Pointer to the quaternion array (4x1) - * @param[in] angle_axis Pointer to the angle_axis array (3x1) + * @param[in] angle_axis Pointer to the angle_axis array (3x1) * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) */ -static inline void quaternionToAngleAxis(const double * q, double * angle_axis, double * jacobian = nullptr) +static inline void quaternionToAngleAxis( + const double * q, double * angle_axis, + double * jacobian = nullptr) { ceres::QuaternionToAngleAxis(q, angle_axis); if (jacobian) { @@ -275,47 +308,51 @@ static inline void quaternionToAngleAxis(const double * q, double * angle_axis, const double & q3 = q[3]; const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; - const double sin_theta = std::sqrt(sin_theta2); + const double sin_theta = std::hypot(q1, q2, q3); const double cos_theta = q0; - if (std::fpclassify(sin_theta) != FP_ZERO) { - const double two_theta = 2.0 * - (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + const double cond = std::pow(sin_theta2, 1.5); + if (std::fpclassify(cond) != FP_ZERO) { + const double two_theta = 2.0 * + (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + const double a = two_theta / sin_theta; + const double b = sin_theta2 * q_sum2; + const double c = two_theta / cond; jacobian_map(0, 0) = -2.0 * q1 / q_sum2; - jacobian_map(0, 1) = - two_theta / sin_theta + - (2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) - - (q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(0, 2) = - (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - - (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 1) = + two_theta / sin_theta + + (2.0 * q0 * q1 * q1) / b - + (q1 * q1 * c); + jacobian_map(0, 2) = + (2.0 * q0 * q1 * q2) / b - + (q1 * q2 * c); jacobian_map(0, 3) = - (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - - (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); - + (2.0 * q0 * q1 * q3) / b - + (q1 * q3 * c); + jacobian_map(1, 0) = -2.0 * q2 / q_sum2; - jacobian_map(1, 1) = - (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - - (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(1, 2) = - two_theta / sin_theta + - (2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) - - (q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(1, 3) = - (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - - (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); - + jacobian_map(1, 1) = + (2.0 * q0 * q1 * q2) / b - + (q1 * q2 * c); + jacobian_map(1, 2) = + two_theta / sin_theta + + (2.0 * q0 * q2 * q2) / b - + (q2 * q2 * c); + jacobian_map(1, 3) = + (2.0 * q0 * q2 * q3) / b - + (q2 * q3 * c); + jacobian_map(2, 0) = -2.0 * q3 / q_sum2; - jacobian_map(2, 1) = - (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - - (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(2, 2) = - (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - - (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 1) = + (2.0 * q0 * q1 * q3) / b - + (q1 * q3 * c); + jacobian_map(2, 2) = + (2.0 * q0 * q2 * q3) / b - + (q2 * q3 * c); jacobian_map(2, 3) = - two_theta / sin_theta + - (2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) - - (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + two_theta / sin_theta + + (2.0 * q0 * q3 * q3) / b - + (q3 * q3 * c); } else { jacobian_map.setZero(); jacobian_map(0, 1) = 2.0; diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 69ed76dab..e6b4b2063 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -41,44 +41,53 @@ #include #include -struct Quat2RPY { - template - bool operator()(const T * const q, T * rpy) const { +struct Quat2RPY +{ + template + bool operator()(const T * const q, T * rpy) const + { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); return true; } - static ceres::CostFunction* Create() { - return (new ceres::AutoDiffCostFunction(new Quat2RPY())); + static ceres::CostFunction * Create() + { + return new ceres::AutoDiffCostFunction(new Quat2RPY()); } }; -struct QuatProd { - template +struct QuatProd +{ + template bool operator()( const T * const q1, const T * const q2, - T * q_out) const { - ceres::QuaternionProduct(q1, q2, q_out); - return true; - } + T * q_out) const + { + ceres::QuaternionProduct(q1, q2, q_out); + return true; + } - static ceres::CostFunction* Create() { - return (new ceres::AutoDiffCostFunction(new QuatProd())); + static ceres::CostFunction * Create() + { + return new ceres::AutoDiffCostFunction(new QuatProd()); } }; -struct Quat2AngleAxis { - template - bool operator()(const T * const q, T * aa) const { - ceres::QuaternionToAngleAxis(q, aa); - return true; - } +struct Quat2AngleAxis +{ + template + bool operator()(const T * const q, T * aa) const + { + ceres::QuaternionToAngleAxis(q, aa); + return true; + } - static ceres::CostFunction* Create() { - return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); + static ceres::CostFunction * Create() + { + return new ceres::AutoDiffCostFunction(new Quat2AngleAxis()); } }; @@ -153,7 +162,7 @@ TEST(Util, quaternion2rpy) q[1] = q_eigen.x(); q[2] = q_eigen.y(); q[3] = q_eigen.z(); - + fuse_core::quaternion2rpy(q, rpy, J_analytic); double * jacobians[1] = {J_autodiff}; @@ -162,10 +171,10 @@ TEST(Util, quaternion2rpy) ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); double rpy_autodiff[3]; quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } @@ -175,7 +184,7 @@ TEST(Util, quaternionProduct) const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); double q_out[4]; - double q1[4] + double q1[4] { q1_eigen.w(), q1_eigen.x(), @@ -183,20 +192,20 @@ TEST(Util, quaternionProduct) q1_eigen.z() }; - double q2[4] + double q2[4] { q2_eigen.w(), q2_eigen.x(), q2_eigen.y(), q2_eigen.z() }; - + // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be - // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion - - fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); double * jacobians[2]; jacobians[0] = J_autodiff_q1; @@ -209,34 +218,36 @@ TEST(Util, quaternionProduct) ceres::CostFunction * quat_prod_cf = QuatProd::Create(); double q_out_autodiff[4]; quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); - + Eigen::Map> J_autodiff_q1_map(jacobians[0]); Eigen::Map> J_autodiff_q2_map(jacobians[1]); // Eigen::Map> J_analytic_q1_map(J_analytic_q1); Eigen::Map> J_analytic_q2_map(J_analytic_q2); - + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); } TEST(Util, quaternionToAngleAxis) { // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation + // The implementation of quaternionToAngleAxis changes slightly between ceres 2.1.0 and 2.2.0. We checked for + // both the versions for the test to pass. { const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); double angle_axis[3]; - double q[4] + double q[4] { q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; - - double J_analytic[12]; + + double J_analytic[12]; double J_autodiff[12]; - - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); double * jacobians[1] = {J_autodiff}; const double * parameters[1] = {q}; @@ -244,10 +255,10 @@ TEST(Util, quaternionToAngleAxis) ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); double angle_axis_autodiff[3]; quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } @@ -255,11 +266,40 @@ TEST(Util, quaternionToAngleAxis) { double angle_axis[3]; double q[4] {1.0, 0.0, 0.0, 0.0}; - - double J_analytic[12]; + + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } + + { + // Test that approximate conversion and jacobian computation work for very small angles that + // could potentially cause underflow. + + double theta = std::pow(std::numeric_limits::min(), 0.75); + double q[4] = {cos(theta / 2.0), sin(theta / 2.0), 0, 0}; + double angle_axis[3]; + double expected[3] = {theta, 0, 0}; + double J_analytic[12]; double J_autodiff[12]; fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + EXPECT_DOUBLE_EQ(angle_axis[0], expected[0]); + EXPECT_DOUBLE_EQ(angle_axis[1], expected[1]); + EXPECT_DOUBLE_EQ(angle_axis[2], expected[2]); double * jacobians[1] = {J_autodiff}; const double * parameters[1] = {q}; @@ -267,10 +307,10 @@ TEST(Util, quaternionToAngleAxis) ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); double angle_axis_autodiff[3]; quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } } From 1383744dc994fdb69519f42753e4d2dcdec9652e Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Tue, 29 Oct 2024 10:26:27 +0100 Subject: [PATCH 114/116] fix typo omnidirectional_3d Co-authored-by: Henry Moore --- fuse_models/src/omnidirectional_3d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index 206199df6..ee925ef40 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -67,7 +67,7 @@ namespace std inline bool isfinite(const fuse_core::Vector3d & vector) { - return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); + return std::isfinite(vector.x()) && std::isfinite(vector.y()) && std::isfinite(vector.z()); } inline bool isNormalized(const Eigen::Quaterniond & q) From 568decf5e3a4375593badf6a2637989ab9cc30e2 Mon Sep 17 00:00:00 2001 From: giacomo Date: Sat, 2 Nov 2024 16:55:46 +0100 Subject: [PATCH 115/116] Missing support for Ceres 2.1.0 Manifold in pose_3d constraint tests --- ...olute_pose_3d_stamped_euler_constraint.cpp | 19 +++++++++-- ...ative_pose_3d_stamped_euler_constraint.cpp | 32 +++++++++++++++++++ 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 2baef4473..5d605f42d 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -246,12 +247,19 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) problem.AddParameterBlock( position_variable->data(), position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); - +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); @@ -365,12 +373,19 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) problem.AddParameterBlock( position_variable->data(), position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif problem.AddParameterBlock( orientation_variable->data(), orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation_variable->localParameterization()); - +#else + orientation_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index 9e701ecbb..ee0724d21 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -282,19 +282,35 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) problem.AddParameterBlock( orientation1->data(), orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif problem.AddParameterBlock( position1->data(), position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS position1->localParameterization()); +#else + position1->manifold()); +#endif problem.AddParameterBlock( orientation2->data(), orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif problem.AddParameterBlock( position2->data(), position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS position2->localParameterization()); +#else + position2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); @@ -501,19 +517,35 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) problem.AddParameterBlock( orientation1->data(), orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif problem.AddParameterBlock( position1->data(), position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS position1->localParameterization()); +#else + position1->manifold()); +#endif problem.AddParameterBlock( orientation2->data(), orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif problem.AddParameterBlock( position2->data(), position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS position2->localParameterization()); +#else + position2->manifold()); +#endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); From 720eaf0f97c67d16bc54f2a191ad9d97a90f55f4 Mon Sep 17 00:00:00 2001 From: giacomo Date: Sat, 2 Nov 2024 16:56:29 +0100 Subject: [PATCH 116/116] add optional tolerance in ExpectCostFunctionsAreEqual --- fuse_constraints/test/cost_function_gtest.hpp | 9 +++++---- fuse_constraints/test/test_normal_prior_pose_3d.cpp | 2 +- .../test/test_normal_prior_pose_3d_euler.cpp | 10 +++++----- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 02cae90e3..9fd98e470 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -53,7 +53,8 @@ */ static void ExpectCostFunctionsAreEqual( const ceres::CostFunction & cost_function, - const ceres::CostFunction & actual_cost_function) + const ceres::CostFunction & actual_cost_function, + const double tol = 1e-16) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); @@ -111,7 +112,7 @@ static void ExpectCostFunctionsAreEqual( parameter_blocks.get(), actual_residuals.get(), nullptr)); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; + EXPECT_NEAR(residuals[i], actual_residuals[i], tol) << "residual id: " << i; } EXPECT_TRUE( @@ -123,11 +124,11 @@ static void ExpectCostFunctionsAreEqual( parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; + EXPECT_NEAR(residuals[i], actual_residuals[i], tol) << "residual : " << i; } for (size_t i = 0; i < num_residuals * num_parameters; ++i) { - EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) + EXPECT_NEAR(jacobians[i], actual_jacobians[i], tol) << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 21a8fffa4..f3ba07a3d 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -89,5 +89,5 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function // and the second argument is the actual cost function - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-14); } diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 9a2dd8853..50301c277 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -90,7 +90,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function // and the second argument is the actual cost function - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-13); } TEST_F( @@ -119,7 +119,7 @@ TEST_F( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-13); } TEST_F( @@ -148,7 +148,7 @@ TEST_F( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-13); } TEST_F( @@ -175,7 +175,7 @@ TEST_F( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-13); } TEST_F( @@ -203,5 +203,5 @@ TEST_F( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); - ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-12); + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function, 1e-13); }