diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 660a7ee2e..8640fe075 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} @@ -116,7 +157,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 09f42f98f..426322c5c 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 @@ -17,6 +18,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 69def70b5..3687d0504 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -49,12 +49,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(); @@ -98,7 +96,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( @@ -110,11 +108,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 3efab9e57..9787d83f6 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 @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -130,7 +131,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()); @@ -201,7 +206,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 f80960862..9ebda7089 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_) @@ -519,9 +519,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(), @@ -551,8 +551,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(