diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 992165cc3..1a4d8c874 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -336,6 +336,7 @@ if(CATKIN_ENABLE_TESTING) # Marginal Constraint Tests catkin_add_gtest(test_marginal_constraint test/test_marginal_constraint.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test ) add_dependencies(test_marginal_constraint ${catkin_EXPORTED_TARGETS} diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.h b/fuse_constraints/include/fuse_constraints/marginal_constraint.h index 6cd59ab7d..a36ce145b 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.h +++ b/fuse_constraints/include/fuse_constraints/marginal_constraint.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -48,14 +49,19 @@ #include #include #include +#include #include #include #include #include #include +#include +#include #include +#include #include +#include #include namespace fuse_constraints @@ -131,7 +137,7 @@ class MarginalConstraint : public fuse_core::Constraint } #else /** - * @brief Read-only access to the variable local parameterizations + * @brief Read-only access to the variable manifolds */ const std::vector& manifolds() const { return manifolds_; } #endif @@ -171,22 +177,72 @@ class MarginalConstraint : public fuse_core::Constraint /** * @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. + * @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 */) + template + void save(Archive& archive, const unsigned int version) const { - archive& boost::serialization::base_object(*this); - archive& A_; - archive& b_; + archive << boost::serialization::base_object(*this); + archive << A_; + archive << b_; #if !CERES_SUPPORTS_MANIFOLDS - archive& local_parameterizations_; + archive << local_parameterizations_; #else - archive& manifolds_; + archive << manifolds_; #endif - archive& x_bar_; + archive << x_bar_; } + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/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 @@ -211,7 +267,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& vari /** * @brief Return the local parameterization of the provided variable */ -inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterization(const fuse_core::Variable& variable) +inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(const fuse_core::Variable& variable) { return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization()); } @@ -220,7 +276,7 @@ inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterizatio /** * @brief Return the manifold of the provided variable */ -inline fuse_core::Manifold::SharedPtr const getManifold(const fuse_core::Variable& variable) +inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& variable) { return fuse_core::Manifold::SharedPtr(variable.manifold()); } @@ -275,5 +331,12 @@ 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_H diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.h b/fuse_constraints/include/fuse_constraints/marginal_cost_function.h index 91bc54959..2a846cd00 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.h +++ b/fuse_constraints/include/fuse_constraints/marginal_cost_function.h @@ -81,10 +81,10 @@ class MarginalCostFunction : public ceres::CostFunction /** * @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 + * @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, @@ -102,7 +102,10 @@ class MarginalCostFunction : public ceres::CostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided variable/parameter * values */ - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override; + bool Evaluate( + double const* const* parameters, + double* residuals, + double** jacobians) const override; private: const std::vector& A_; //!< The A matrices of the marginal cost diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index d57e82c90..ed6f334ef 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -57,10 +57,8 @@ void MarginalConstraint::print(std::ostream& stream) const Eigen::IOFormat indent(4, 0, ", ", "\n", " [", "]"); for (size_t i = 0; i < A().size(); ++i) { - stream << " A[" << i << "]:\n" - << A()[i].format(indent) << "\n" - << " x_bar[" << i << "]:\n" - << x_bar()[i].format(indent) << "\n"; + stream << " A[" << i << "]:\n" << A()[i].format(indent) << "\n" + << " x_bar[" << i << "]:\n" << x_bar()[i].format(indent) << "\n"; } stream << " b:\n" << b().format(indent) << "\n"; diff --git a/fuse_constraints/src/marginal_cost_function.cpp b/fuse_constraints/src/marginal_cost_function.cpp index 20a559d5b..c94452a30 100644 --- a/fuse_constraints/src/marginal_cost_function.cpp +++ b/fuse_constraints/src/marginal_cost_function.cpp @@ -79,7 +79,10 @@ MarginalCostFunction::MarginalCostFunction( } #endif -bool MarginalCostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const +bool MarginalCostFunction::Evaluate( + double const* const* parameters, + double* residuals, + double** jacobians) const { // Compute cost Eigen::Map residuals_map(residuals, num_residuals()); diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 76d6e4b80..8dd784470 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -103,8 +103,10 @@ UuidOrdering computeEliminationOrder( } // Construct the CCOLAMD input structures - auto recommended_size = - ccolamd_recommended(variable_constraints.size(), constraint_order.size(), variable_order.size()); + auto recommended_size = ccolamd_recommended( + variable_constraints.size(), + constraint_order.size(), + variable_order.size()); auto A = std::vector(recommended_size); auto p = std::vector(variable_order.size() + 1); diff --git a/fuse_constraints/test/legacy_marginal_version0.txt b/fuse_constraints/test/legacy_marginal_version0.txt new file mode 100644 index 000000000..9f1b61640 --- /dev/null +++ b/fuse_constraints/test/legacy_marginal_version0.txt @@ -0,0 +1,3 @@ +22 serialization::archive 17 1 0 +0 0 0 4 test 68a8f481-d9e2-434e-96d8-d9c52fac6603 0 0 1 0 d0344d9a-5584-5a24-b972-ea410b25e499 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 c70ecd727..dbd63a156 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -52,6 +52,7 @@ #include #include + TEST(AbsoluteConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. @@ -79,7 +80,8 @@ TEST(AbsoluteConstraint, Constructor) mean << 3.0; fuse_core::Matrix1d cov; cov << 1.0; - EXPECT_NO_THROW(fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov)); + EXPECT_NO_THROW( + fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov)); } { fuse_variables::Position2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("rosie")); @@ -87,7 +89,8 @@ TEST(AbsoluteConstraint, Constructor) mean << 1.0, 2.0; fuse_core::Matrix2d cov; cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW(fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov)); + EXPECT_NO_THROW( + fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov)); } { fuse_variables::Position3DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("clank")); @@ -95,7 +98,8 @@ TEST(AbsoluteConstraint, Constructor) mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - EXPECT_NO_THROW(fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov)); + EXPECT_NO_THROW( + fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov)); } { fuse_variables::VelocityAngular2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("gort")); @@ -124,7 +128,7 @@ TEST(AbsoluteConstraint, PartialMeasurement) mean << 3.0, 1.0; fuse_core::Matrix2d cov; cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector { 2, 0 }; + auto indices = std::vector{2, 0}; EXPECT_NO_THROW( fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices)); } @@ -142,7 +146,8 @@ TEST(AbsoluteConstraint, Covariance) fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix2d expected_sqrt_info; - expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547; + expected_sqrt_info << 1.002509414234171, -0.050125470711709, + 0.000000000000000, 0.707106781186547; fuse_core::Matrix2d expected_cov = cov; // Compare EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); @@ -155,7 +160,7 @@ TEST(AbsoluteConstraint, Covariance) mean << 3.0, 1.0; fuse_core::Matrix2d cov; cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector { 2, 0 }; + auto indices = std::vector{2, 0}; fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices); // Define the expected matrices fuse_core::Vector3d expected_mean; @@ -163,8 +168,8 @@ TEST(AbsoluteConstraint, Covariance) fuse_core::Matrix3d expected_cov; expected_cov << 1.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0; fuse_core::MatrixXd expected_sqrt_info(2, 3); - expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, 1.000000000000000, - 0.000000000000000, 0.000000000000000; + expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, + 1.000000000000000, 0.000000000000000, 0.000000000000000; // Compare EXPECT_TRUE(expected_mean.isApprox(constraint.mean(), 1.0e-9)); EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); @@ -188,8 +193,11 @@ TEST(AbsoluteConstraint, Optimization) mean << 1.0, 2.0; fuse_core::Matrix2d cov; cov << 1.0, 0.1, 0.1, 2.0; - auto constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared("test", *variable, mean, cov); + auto constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + "test", + *variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -202,10 +210,12 @@ TEST(AbsoluteConstraint, Optimization) #else variable->manifold()); #endif - std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -214,7 +224,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(1.0, variable->x(), 1.0e-5); EXPECT_NEAR(2.0, variable->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -239,15 +249,26 @@ TEST(AbsoluteConstraint, Optimization) fuse_core::Vector3d mean1; mean1 << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov1; - cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1); + cov1 << 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0; + auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + "test", + *var, + mean1, + cov1); fuse_core::Vector2d mean2; mean2 << 4.0, 2.0; fuse_core::Matrix2d cov2; - cov2 << 1.0, 0.0, 0.0, 1.0; - auto indices2 = std::vector { 2, 0 }; - auto constraint2 = - fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2); + cov2 << 1.0, 0.0, + 0.0, 1.0; + auto indices2 = std::vector{2, 0}; + auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + "test", + *var, + mean2, + cov2, + indices2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -262,8 +283,14 @@ TEST(AbsoluteConstraint, Optimization) #endif std::vector parameter_blocks; parameter_blocks.push_back(var->data()); - problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); - problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint1->costFunction(), + constraint1->lossFunction(), + parameter_blocks); + problem.AddResidualBlock( + constraint2->costFunction(), + constraint2->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -273,7 +300,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(2.0, var->y(), 1.0e-5); EXPECT_NEAR(3.5, var->z(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(var->data(), var->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -282,7 +309,9 @@ TEST(AbsoluteConstraint, Optimization) covariance.GetCovarianceBlock(var->data(), var->data(), covariance_vector.data()); fuse_core::Matrix3d actual_cov(covariance_vector.data()); fuse_core::Matrix3d expected_cov; - expected_cov << 0.5, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.5; + expected_cov << 0.5, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 0.5; EXPECT_TRUE(expected_cov.isApprox(actual_cov, 1.0e-9)); } } @@ -299,19 +328,28 @@ TEST(AbsoluteConstraint, PartialOptimization) fuse_core::Vector2d mean1; mean1 << 1.0, 3.0; fuse_core::Matrix2d cov1; - cov1 << 1.0, 0.0, 0.0, 1.0; - std::vector indices1 = { fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X }; - auto constraint1 = - fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1, indices1); + cov1 << 1.0, 0.0, + 0.0, 1.0; + std::vector indices1 = {fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X}; + auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + "test", + *var, + mean1, + cov1, + indices1); // Create another constraint for the second index fuse_core::Vector1d mean2; mean2 << 2.0; fuse_core::Matrix1d cov2; cov2 << 1.0; - std::vector indices2 = { fuse_variables::Position3DStamped::Y }; - auto constraint2 = - fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2); + std::vector indices2 = {fuse_variables::Position3DStamped::Y}; + auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + "test", + *var, + mean2, + cov2, + indices2); // Build the problem ceres::Problem::Options problem_options; @@ -327,8 +365,14 @@ TEST(AbsoluteConstraint, PartialOptimization) #endif std::vector parameter_blocks; parameter_blocks.push_back(var->data()); - problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); - problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint1->costFunction(), + constraint1->lossFunction(), + parameter_blocks); + problem.AddResidualBlock( + constraint2->costFunction(), + constraint2->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -343,15 +387,20 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) { // Optimize a single variable and single constraint, verify the expected value and covariance are generated. // Create a variable - auto variable = - fuse_variables::Orientation2DStamped::make_shared(ros::Time(1234, 5678), fuse_core::uuid::generate("tiktok")); + auto variable = fuse_variables::Orientation2DStamped::make_shared( + ros::Time(1234, 5678), + fuse_core::uuid::generate("tiktok")); variable->setYaw(0.7); // Create an absolute constraint fuse_core::Vector1d mean; mean << 7.0; fuse_core::Matrix1d cov; cov << 0.10; - auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared("test", *variable, mean, cov); + auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -366,7 +415,10 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) #endif std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -374,7 +426,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) // Check EXPECT_NEAR(7.0 - 2 * M_PI, variable->getYaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -416,7 +468,7 @@ TEST(AbsoluteConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp index f7a48cc26..4c060a7bc 100644 --- a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -39,10 +39,10 @@ #include #include -#include #include #include #include +#include #include #include @@ -51,6 +51,7 @@ using fuse_constraints::AbsoluteOrientation2DStampedConstraint; using fuse_variables::Orientation2DStamped; + TEST(AbsoluteOrientation2DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. @@ -74,7 +75,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, Covariance) // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix1d expected_sqrt_info; - expected_sqrt_info << 1.0; + expected_sqrt_info << 1.0; fuse_core::Matrix1d expected_cov = cov; // Compare @@ -95,7 +96,11 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) fuse_core::Matrix1d cov; cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; @@ -112,7 +117,10 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -124,16 +132,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) EXPECT_NEAR(1.0, orientation_variable->getYaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -156,7 +162,11 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) fuse_core::Matrix1d cov; cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; @@ -173,7 +183,10 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -185,16 +198,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) EXPECT_NEAR(0.0, orientation_variable->getYaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -217,7 +228,11 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) fuse_core::Matrix1d cov; cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; @@ -234,7 +249,10 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -247,16 +265,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -279,7 +295,11 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) fuse_core::Matrix1d cov; cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; @@ -296,7 +316,10 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -308,16 +331,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -356,7 +377,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); 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 8293cd9c2..8860c4b74 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -126,7 +126,6 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) #else position_variable->manifold()); #endif - std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); @@ -217,14 +216,6 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) 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 problem.AddParameterBlock( position_variable->data(), position_variable->size(), @@ -233,7 +224,14 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) #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 a16257426..486232ed5 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -47,9 +47,10 @@ #include #include -using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; + TEST(AbsolutePose3DStampedConstraint, Constructor) { @@ -62,12 +63,12 @@ TEST(AbsolutePose3DStampedConstraint, Constructor) // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; EXPECT_NO_THROW( AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov)); @@ -84,24 +85,23 @@ TEST(AbsolutePose3DStampedConstraint, Covariance) // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, - -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT fuse_core::Matrix6d expected_cov = cov; // Compare @@ -129,24 +129,24 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov; - cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, 0.5, 0.2, 4.0, - 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; - - auto constraint = - AbsolutePose3DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, mean, cov); + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + + auto constraint = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - 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(), @@ -155,12 +155,23 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) #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()); - problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -177,7 +188,7 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); @@ -190,15 +201,11 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), - orientation_variable->data(), - cov_or_or.data()); + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), - orientation_variable->data(), - cov_pos_or.data()); + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -206,8 +213,13 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; - expected_covariance << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, - 0.5, 0.2, 4.0, 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + expected_covariance << + 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); } @@ -223,12 +235,12 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov); @@ -253,7 +265,7 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp index 229d16d05..2c91152b0 100644 --- a/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp +++ b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp @@ -192,13 +192,21 @@ TEST(Fixed3DLandmarkConstraint, Optimization) ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(position_variable->data(), position_variable->size(), position_variable->localParameterization()); problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), orientation_variable->localParameterization()); problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), calibration_variable->localParameterization()); - +#else + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->manifold()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->manifold()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); @@ -319,12 +327,21 @@ TEST(Fixed3DLandmarkConstraint, OptimizationScaledMarker) ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(position_variable->data(), position_variable->size(), position_variable->localParameterization()); problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), orientation_variable->localParameterization()); problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), calibration_variable->localParameterization()); +#else + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->manifold()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->manifold()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); @@ -422,13 +439,21 @@ TEST(Fixed3DLandmarkConstraint, OptimizationPoints) ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(position_variable->data(), position_variable->size(), position_variable->localParameterization()); problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), orientation_variable->localParameterization()); problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), calibration_variable->localParameterization()); - +#else + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->manifold()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->manifold()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); @@ -526,12 +551,21 @@ TEST(Fixed3DLandmarkConstraint, MultiViewOptimization) orientation_vars[i]->y() = -0.189; orientation_vars[i]->z() = 0.239; +#if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(position_vars[i]->data(), position_vars[i]->size(), position_vars[i]->localParameterization()); problem.AddParameterBlock(orientation_vars[i]->data(), orientation_vars[i]->size(), orientation_vars[i]->localParameterization()); problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), calibration_variable->localParameterization()); +#else + problem.AddParameterBlock(position_vars[i]->data(), position_vars[i]->size(), + position_vars[i]->manifold()); + problem.AddParameterBlock(orientation_vars[i]->data(), orientation_vars[i]->size(), + orientation_vars[i]->manifold()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->manifold()); +#endif std::vector parameter_blocks; parameter_blocks.push_back(position_vars[i]->data()); diff --git a/fuse_constraints/test/test_marginal_constraint.cpp b/fuse_constraints/test/test_marginal_constraint.cpp index a40f85028..a9efb96b3 100644 --- a/fuse_constraints/test/test_marginal_constraint.cpp +++ b/fuse_constraints/test/test_marginal_constraint.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -41,9 +42,11 @@ #include +#include #include #include + TEST(MarginalConstraint, OneVariable) { // Create a marginal constraint with one variable, no local parameterizations @@ -61,8 +64,13 @@ TEST(MarginalConstraint, OneVariable) fuse_core::Vector1d b; b << 3.0; - auto constraint = - fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); + auto constraint = fuse_constraints::MarginalConstraint( + "test", + variables.begin(), + variables.end(), + A.begin(), + A.end(), + b); auto cost_function = constraint.costFunction(); @@ -71,10 +79,10 @@ TEST(MarginalConstraint, OneVariable) x1.y() = 6.0; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data() }; + std::vector variable_values = {x1.data()}; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); - std::vector actual_jacobians = { actual_jacobian1.data() }; + std::vector actual_jacobians = {actual_jacobian1.data()}; cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians @@ -115,8 +123,13 @@ TEST(MarginalConstraint, TwoVariables) fuse_core::Vector1d b; b << 9.0; - auto constraint = - fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); + auto constraint = fuse_constraints::MarginalConstraint( + "test", + variables.begin(), + variables.end(), + A.begin(), + A.end(), + b); auto cost_function = constraint.costFunction(); @@ -128,11 +141,11 @@ TEST(MarginalConstraint, TwoVariables) x2.y() = 18.0; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data(), x2.data() }; + std::vector variable_values = {x1.data(), x2.data()}; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); fuse_core::MatrixXd actual_jacobian2(1, 2); - std::vector actual_jacobians = { actual_jacobian1.data(), actual_jacobian2.data() }; + std::vector actual_jacobians = {actual_jacobian1.data(), actual_jacobian2.data()}; cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians @@ -169,8 +182,13 @@ TEST(MarginalConstraint, LocalParameterization) fuse_core::Vector1d b; b << 8.0; - auto constraint = - fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); + auto constraint = fuse_constraints::MarginalConstraint( + "test", + variables.begin(), + variables.end(), + A.begin(), + A.end(), + b); auto cost_function = constraint.costFunction(); // Update the variable value @@ -181,10 +199,10 @@ TEST(MarginalConstraint, LocalParameterization) x1.z() = 0.526043; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data() }; + std::vector variable_values = {x1.data()}; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 4); - std::vector actual_jacobians = { actual_jacobian1.data() }; + std::vector actual_jacobians = {actual_jacobian1.data()}; cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians @@ -222,8 +240,13 @@ TEST(MarginalConstraint, Serialization) fuse_core::Vector1d b; b << 8.0; - auto expected = - fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); + auto expected = fuse_constraints::MarginalConstraint( + "test", + variables.begin(), + variables.end(), + A.begin(), + A.end(), + b); // Serialize the constraint into an archive std::stringstream stream; @@ -246,8 +269,8 @@ 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 - using ExpectedLocalParam = fuse_variables::Orientation3DLocalParameterization; #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) { @@ -255,16 +278,72 @@ TEST(MarginalConstraint, Serialization) 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]); + auto actual_derived = std::dynamic_pointer_cast(actual.manifolds()[i]); EXPECT_TRUE(static_cast(actual_derived)); } #endif } -int main(int argc, char** argv) +#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(ros::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 + +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 7e03994b6..1e498d4f6 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -53,6 +53,7 @@ #include #include + TEST(RelativeConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. @@ -83,7 +84,8 @@ TEST(RelativeConstraint, Constructor) delta << 3.0; fuse_core::Matrix1d cov; cov << 1.0; - EXPECT_NO_THROW(fuse_constraints::RelativeOrientation2DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW( + fuse_constraints::RelativeOrientation2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { fuse_variables::Position2DStamped x1(ros::Time(1234, 5678), fuse_core::uuid::generate("rosie")); @@ -92,7 +94,8 @@ TEST(RelativeConstraint, Constructor) delta << 1.0, 2.0; fuse_core::Matrix2d cov; cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW(fuse_constraints::RelativePosition2DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW( + fuse_constraints::RelativePosition2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { fuse_variables::Position3DStamped x1(ros::Time(1234, 5678), fuse_core::uuid::generate("clank")); @@ -101,7 +104,8 @@ TEST(RelativeConstraint, Constructor) delta << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.3, 2.0, 0.3, 0.2, 0.3, 3.0; - EXPECT_NO_THROW(fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW( + fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov)); } { fuse_variables::VelocityAngular2DStamped x1(ros::Time(1234, 5678), fuse_core::uuid::generate("gort")); @@ -120,7 +124,8 @@ TEST(RelativeConstraint, Constructor) delta << 1.0, 2.0; fuse_core::Matrix2d cov; cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW(fuse_constraints::RelativeVelocityLinear2DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW( + fuse_constraints::RelativeVelocityLinear2DStampedConstraint constraint("test", x1, x2, delta, cov)); } } @@ -132,7 +137,7 @@ TEST(RelativeConstraint, PartialMeasurement) delta << 3.0, 1.0; fuse_core::Matrix2d cov; cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector { 2, 0 }; + auto indices = std::vector{2, 0}; EXPECT_NO_THROW( fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, indices)); } @@ -151,7 +156,8 @@ TEST(RelativeConstraint, Covariance) fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, delta, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix2d expected_sqrt_info; - expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547; + expected_sqrt_info << 1.002509414234171, -0.050125470711709, + 0.000000000000000, 0.707106781186547; fuse_core::Matrix2d expected_cov = cov; // Compare EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); @@ -165,7 +171,7 @@ TEST(RelativeConstraint, Covariance) delta << 3.0, 1.0; fuse_core::Matrix2d cov; cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector { 2, 0 }; + auto indices = std::vector{2, 0}; fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, indices); // Define the expected matrices fuse_core::Vector3d expected_delta; @@ -173,8 +179,8 @@ TEST(RelativeConstraint, Covariance) fuse_core::Matrix3d expected_cov; expected_cov << 1.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0; fuse_core::MatrixXd expected_sqrt_info(2, 3); - expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, 1.000000000000000, - 0.000000000000000, 0.000000000000000; + expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, + 1.000000000000000, 0.000000000000000, 0.000000000000000; // Compare EXPECT_TRUE(expected_delta.isApprox(constraint.delta(), 1.0e-9)); EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); @@ -204,14 +210,22 @@ TEST(RelativeConstraint, Optimization) mean << 1.0, 2.0; fuse_core::Matrix2d cov1; cov1 << 1.0, 0.1, 0.1, 2.0; - auto prior = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared("test", *x1, mean, cov1); + auto prior = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + "test", + *x1, + mean, + cov1); // Create an relative constraint fuse_core::Vector2d delta; delta << 0.1, 0.2; fuse_core::Matrix2d cov2; cov2 << 1.0, 0.0, 0.0, 2.0; - auto relative = - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint::make_shared("test", *x1, *x2, delta, cov2); + auto relative = fuse_constraints::RelativeAccelerationLinear2DStampedConstraint::make_shared( + "test", + *x1, + *x2, + delta, + cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -234,11 +248,17 @@ TEST(RelativeConstraint, Optimization) #endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(x1->data()); relative_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -249,7 +269,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(1.1, x2->x(), 1.0e-5); EXPECT_NEAR(2.2, x2->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -277,11 +297,15 @@ TEST(RelativeConstraint, Optimization) // Optimize a two-variable system with a prior on the first variable and a relative constraint connecting the two. // Verify the expected value and covariance are generated. // Create the variables - auto x1 = fuse_variables::Position3DStamped::make_shared(ros::Time(1234, 5678), fuse_core::uuid::generate("t1000")); + auto x1 = fuse_variables::Position3DStamped::make_shared( + ros::Time(1234, 5678), + fuse_core::uuid::generate("t1000")); x1->x() = 10.7; x1->y() = -3.2; x1->z() = 0.4; - auto x2 = fuse_variables::Position3DStamped::make_shared(ros::Time(1235, 5678), fuse_core::uuid::generate("t1000")); + auto x2 = fuse_variables::Position3DStamped::make_shared( + ros::Time(1235, 5678), + fuse_core::uuid::generate("t1000")); x2->x() = -4.2; x2->y() = 1.9; x2->z() = 19.2; @@ -290,21 +314,35 @@ TEST(RelativeConstraint, Optimization) mean1 << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - auto c1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *x1, mean1, cov1); + auto c1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + "test", + *x1, + mean1, + cov1); // Create an relative constraint fuse_core::Vector3d delta2; delta2 << 0.1, 0.2, 0.3; fuse_core::Matrix3d cov2; cov2 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto c2 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared("test", *x1, *x2, delta2, cov2); + auto c2 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared( + "test", + *x1, + *x2, + delta2, + cov2); // Create an partial relative constraint fuse_core::Vector2d delta3; delta3 << 0.1, 0.2; fuse_core::Matrix2d cov3; cov3 << 1.0, 0.0, 0.0, 3.0; - auto indices3 = std::vector { 2, 0 }; - auto c3 = - fuse_constraints::RelativePosition3DStampedConstraint::make_shared("test", *x1, *x2, delta3, cov3, indices3); + auto indices3 = std::vector{2, 0}; + auto c3 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared( + "test", + *x1, + *x2, + delta3, + cov3, + indices3); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -327,15 +365,24 @@ TEST(RelativeConstraint, Optimization) #endif std::vector c1_parameter_blocks; c1_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock(c1->costFunction(), c1->lossFunction(), c1_parameter_blocks); + problem.AddResidualBlock( + c1->costFunction(), + c1->lossFunction(), + c1_parameter_blocks); std::vector c2_parameter_blocks; c2_parameter_blocks.push_back(x1->data()); c2_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock(c2->costFunction(), c2->lossFunction(), c2_parameter_blocks); + problem.AddResidualBlock( + c2->costFunction(), + c2->lossFunction(), + c2_parameter_blocks); std::vector c3_parameter_blocks; c3_parameter_blocks.push_back(x1->data()); c3_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock(c3->costFunction(), c3->lossFunction(), c3_parameter_blocks); + problem.AddResidualBlock( + c3->costFunction(), + c3->lossFunction(), + c3_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -348,7 +395,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(2.2, x2->y(), 1.0e-5); EXPECT_NEAR(3.15, x2->z(), 1.0e-5); // Compute the marginal covariances - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -376,22 +423,35 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) // Optimize a two-variable system with a prior on the first variable and a relative constraint connecting the two. // Verify the expected value and covariance are generated. // Create the variables - auto x1 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1234, 5678), fuse_core::uuid::generate("t800")); + auto x1 = fuse_variables::Orientation2DStamped::make_shared( + ros::Time(1234, 5678), + fuse_core::uuid::generate("t800")); x1->setYaw(0.7); - auto x2 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 5678), fuse_core::uuid::generate("t800")); + auto x2 = fuse_variables::Orientation2DStamped::make_shared( + ros::Time(1235, 5678), + fuse_core::uuid::generate("t800")); x2->setYaw(-2.2); // Create an absolute constraint fuse_core::Vector1d mean; mean << 1.0; fuse_core::Matrix1d cov1; cov1 << 2.0; - auto prior = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared("test", *x1, mean, cov1); + auto prior = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *x1, + mean, + cov1); // Create an relative constraint fuse_core::Vector1d delta; delta << 0.1; fuse_core::Matrix1d cov2; cov2 << 1.0; - auto relative = fuse_constraints::RelativeOrientation2DStampedConstraint::make_shared("test", *x1, *x2, delta, cov2); + auto relative = fuse_constraints::RelativeOrientation2DStampedConstraint::make_shared( + "test", + *x1, + *x2, + delta, + cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; @@ -414,11 +474,17 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) #endif std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(x1->data()); relative_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -427,7 +493,7 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) EXPECT_NEAR(1.0, x1->getYaw(), 1.0e-5); EXPECT_NEAR(1.1, x2->getYaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -481,7 +547,7 @@ TEST(RelativeConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); 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 90ab420dd..baf2f80e3 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -48,10 +48,11 @@ #include #include -using fuse_constraints::AbsolutePose2DStampedConstraint; -using fuse_constraints::RelativePose2DStampedConstraint; using fuse_variables::Orientation2DStamped; using fuse_variables::Position2DStamped; +using fuse_constraints::AbsolutePose2DStampedConstraint; +using fuse_constraints::RelativePose2DStampedConstraint; + TEST(RelativePose2DStampedConstraint, Constructor) { @@ -79,11 +80,19 @@ TEST(RelativePose2DStampedConstraint, Covariance) delta << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - RelativePose2DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov); + RelativePose2DStampedConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov); // Define the expected matrices (used Octave to compute sqrt_info) fuse_core::Matrix3d expected_sqrt_info; - expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, 0.000000000000000, 0.712470499879096, - -0.071247049987910, 0.000000000000000, 0.000000000000000, 0.577350269189626; + expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, + 0.000000000000000, 0.712470499879096, -0.071247049987910, + 0.000000000000000, 0.000000000000000, 0.577350269189626; fuse_core::Matrix3d expected_cov = cov; // Compare EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); @@ -110,7 +119,12 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) mean1 << 0.0, 0.0, 0.0; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto prior = AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); + auto prior = AbsolutePose2DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean1, + cov1); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector3d delta2; delta2 << 1.0, 0.0, 0.0; @@ -163,13 +177,19 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -183,7 +203,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(0.0, orientation2->getYaw(), 1.0e-5); // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -212,7 +232,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -264,15 +284,20 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) mean1 << 0.0, 0.0, 0.0; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto prior = AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); + auto prior = AbsolutePose2DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean1, + cov1); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector2d delta1; delta1 << 1.0, 0.0; fuse_core::Matrix2d cov_rel1; cov_rel1 << 1.0, 0.0, 0.0, 1.0; - std::vector axes_lin1 = { fuse_variables::Position2DStamped::X }; - std::vector axes_ang1 = { fuse_variables::Orientation2DStamped::YAW }; + std::vector axes_lin1 = {fuse_variables::Position2DStamped::X}; + std::vector axes_ang1 = {fuse_variables::Orientation2DStamped::YAW}; auto relative1 = RelativePose2DStampedConstraint::make_shared( "test", *position1, @@ -289,7 +314,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) delta2 << 0.0; fuse_core::Matrix1d cov_rel2; cov_rel2 << 1.0; - std::vector axes_lin2 = { fuse_variables::Position2DStamped::Y }; + std::vector axes_lin2 = {fuse_variables::Position2DStamped::Y}; std::vector axes_ang2 = {}; auto relative2 = RelativePose2DStampedConstraint::make_shared( "test", @@ -342,15 +367,24 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock(relative1->costFunction(), relative1->lossFunction(), relative_parameter_blocks); - problem.AddResidualBlock(relative2->costFunction(), relative2->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock( + relative1->costFunction(), + relative1->lossFunction(), + relative_parameter_blocks); + problem.AddResidualBlock( + relative2->costFunction(), + relative2->lossFunction(), + relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -367,7 +401,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -399,7 +433,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -466,7 +500,7 @@ TEST(RelativePose2DStampedConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); 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 3c4eeced1..45513da31 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -48,10 +48,11 @@ #include #include -using fuse_constraints::AbsolutePose3DStampedConstraint; -using fuse_constraints::RelativePose3DStampedConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::RelativePose3DStampedConstraint; + TEST(RelativePose3DStampedConstraint, Constructor) { @@ -66,12 +67,12 @@ TEST(RelativePose3DStampedConstraint, Constructor) // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; EXPECT_NO_THROW( RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov)); @@ -89,24 +90,30 @@ TEST(RelativePose3DStampedConstraint, Covariance) // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; - - RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov); + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + + RelativePose3DStampedConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, - -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT fuse_core::Matrix6d expected_cov = cov; // Compare @@ -145,7 +152,12 @@ TEST(RelativePose3DStampedConstraint, Optimization) fuse_core::Vector7d mean_origin; mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); - auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector7d mean_delta; @@ -199,13 +211,19 @@ TEST(RelativePose3DStampedConstraint, Optimization) std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -230,7 +248,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -254,15 +272,20 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; - expected_covariance << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector > covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -286,8 +309,13 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; - expected_covariance << 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + expected_covariance << + 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); } @@ -306,12 +334,12 @@ TEST(RelativePose3DStampedConstraint, Serialization) // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) fuse_core::Matrix6d cov; - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); @@ -336,7 +364,7 @@ TEST(RelativePose3DStampedConstraint, Serialization) EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } -int main(int argc, char** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index d0faf3e09..7cb841059 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -45,6 +45,7 @@ add_library(${PROJECT_NAME} src/graph.cpp src/graph_deserializer.cpp src/loss.cpp + src/manifold_adapter.cpp src/serialization.cpp src/timestamp_manager.cpp src/transaction.cpp @@ -508,6 +509,7 @@ if(CATKIN_ENABLE_TESTING) # Variable tests catkin_add_gtest(test_variable test/test_variable.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test ) add_dependencies(test_variable ${catkin_EXPORTED_TARGETS} diff --git a/fuse_core/include/fuse_core/local_parameterization.h b/fuse_core/include/fuse_core/local_parameterization.h index 464041173..7fc1995ef 100644 --- a/fuse_core/include/fuse_core/local_parameterization.h +++ b/fuse_core/include/fuse_core/local_parameterization.h @@ -45,6 +45,8 @@ // version 2.2.0, see // https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc #include +#else +#include #endif namespace fuse_core @@ -60,7 +62,7 @@ namespace fuse_core * See the Ceres documentation for more details. http://ceres-solver.org/nnls_modeling.html#localparameterization */ class LocalParameterization -// extend ceres LocalParameterization if we are <= 2.1 +// extend ceres LocalParameterization if our version of Ceres does not support Manifolds #if !CERES_SUPPORTS_MANIFOLDS : public ceres::LocalParameterization #endif @@ -69,40 +71,29 @@ class LocalParameterization FUSE_SMART_PTR_ALIASES_ONLY(LocalParameterization); /** - * @brief Generalization of the subtraction operation - * - * Minus(x, y) -> y_minus_x - * - * with the conditions that: - * - Minus(x, x) -> 0 - * - if Plus(y, delta) -> x, then Minus(x, y) -> delta - * - * @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 + * @brief Destroy the Local Parameterization object */ - virtual bool Minus(const double* x, const double* y, double* y_minus_x) const = 0; + virtual ~LocalParameterization() = default; /** - * @brief The jacobian of Minus(x, y) w.r.t y at y = x, i.e - * - * (D_1 Minus) (y, y) + * @brief Size of x * - * @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 GlobalSize() - * @return True if successful, false otherwise + * @return int Size of x. */ - virtual bool ComputeMinusJacobian(const double* x, double* jacobian) const = 0; + virtual int GlobalSize() const = 0; -#if CERES_SUPPORTS_MANIFOLDS - virtual ~LocalParameterization() = default; + /** + * @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() @@ -114,13 +105,44 @@ class LocalParameterization /** * @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 + * @return */ virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + /** + * @brief Generalization of the subtraction operation + * + * Minus(x, y) -> y_minus_x + * + * with the conditions that: + * - Minus(x, x) -> 0 + * - if Plus(x, delta) -> y, then Minus(x, y) -> delta + * + * @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* x, const double* y, double* y_minus_x) const = 0; + + /** + * @brief The jacobian of Minus(x, y) w.r.t y at y = x, i.e + * + * (D_1 Minus) (y, 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 GlobalSize() + * @return True if successful, false otherwise + */ + virtual bool ComputeMinusJacobian(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 * @@ -140,28 +162,27 @@ class LocalParameterization 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; } - Eigen::MatrixXd jacobian(GlobalSize(), LocalSize()); + Matrix jacobian(GlobalSize(), LocalSize()); if (!ComputeJacobian(x, jacobian.data())) { return false; } - Eigen::Map(local_matrix, num_rows, LocalSize()) = - Eigen::Map(global_matrix, num_rows, GlobalSize()) * jacobian; + MatrixRef(local_matrix, num_rows, LocalSize()) = + ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; return true; } - - // Size of x. - virtual int GlobalSize() const = 0; - // Size of delta. - virtual int LocalSize() const = 0; - #endif + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -172,7 +193,7 @@ class LocalParameterization * @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 + template void serialize(Archive& /* archive */, const unsigned int /* version */) { } diff --git a/fuse_core/include/fuse_core/manifold.h b/fuse_core/include/fuse_core/manifold.h index 2aff0cdb2..a9ece89d9 100644 --- a/fuse_core/include/fuse_core/manifold.h +++ b/fuse_core/include/fuse_core/manifold.h @@ -37,15 +37,16 @@ #include #if CERES_SUPPORTS_MANIFOLDS + #include #include #include -// Local parameterizations got marked as deprecated in favour of Manifold in +// 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 they got removed in 2.2.0, see +// and Local parameterizations were removed in 2.2.0, see // https://github.com/ceres-solver/ceres-solver/commit/68c53bb39552cd4abfd6381df08638285f7386b3 #include @@ -54,8 +55,24 @@ namespace fuse_core /** * @brief The Manifold interface definition. * - * This class extends the Ceres Manifold class but adds methods to match the - * fuse::LocalParameterization API + * 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 @@ -65,10 +82,23 @@ class Manifold : public ceres::Manifold public: FUSE_SMART_PTR_ALIASES_ONLY(Manifold); - // Dimension of the ambient space in which the manifold is embedded. + /** + * @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; - // Dimension of the manifold/tangent space. + /** + * @brief Dimension of the manifold/tangent space. + * + * @return int Dimension of the manifold/tangent space. + */ virtual int TangentSize() const = 0; /** @@ -105,7 +135,7 @@ class Manifold : public ceres::Manifold * * with the conditions that: * - Minus(x, x) -> 0 - * - if Plus(y, delta) -> x, then Minus(y, x) -> delta + * - 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. diff --git a/fuse_core/include/fuse_core/manifold_adapter.h b/fuse_core/include/fuse_core/manifold_adapter.h index 3cf3ef595..13a0a114b 100644 --- a/fuse_core/include/fuse_core/manifold_adapter.h +++ b/fuse_core/include/fuse_core/manifold_adapter.h @@ -37,31 +37,67 @@ #include -#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 * - * @param[in] local_parameterization fuse::LocalParameterization + * 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_ = *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); } - // Dimension of the ambient space in which the manifold is embedded. + /** + * @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(); } - // Dimension of the manifold/tangent space. + /** + * @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(); } /** @@ -103,6 +139,9 @@ class ManifoldAdapter : public fuse_core::Manifold * 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. @@ -128,11 +167,34 @@ class ManifoldAdapter : public fuse_core::Manifold } private: - std::unique_ptr local_parameterization_; + // 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_H diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h index 895f78f48..fff6d3f8d 100644 --- a/fuse_core/include/fuse_core/variable.h +++ b/fuse_core/include/fuse_core/variable.h @@ -49,6 +49,7 @@ #include #include + /** * @brief Implementation of the clone() member function for derived classes * @@ -63,11 +64,13 @@ * @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 + * @brief Implementation of the serialize() and deserialize() member functions for derived classes * * Usage: * @code{.cpp} @@ -79,18 +82,28 @@ * } * @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; } +#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; \ + } /** - * @brief Implements the type() member function using the suggested - * implementation + * @brief Implements the type() member function using the suggested implementation * - * Also creates a static detail::type() function that may be used without an - * object instance + * Also creates a static detail::type() function that may be used without an object instance * * Usage: * @code{.cpp} @@ -102,19 +115,21 @@ * } * @endcode */ -#define FUSE_VARIABLE_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ +#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(); } + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ + } /** - * @brief Convenience function that creates the required pointer aliases, - * clone() method, and type() method + * @brief Convenience function that creates the required pointer aliases, clone() method, and type() method * * Usage: * @code{.cpp} @@ -126,16 +141,15 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ +#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__) /** - * @brief Convenience function that creates the required pointer aliases, - * clone() method, and type() method for derived Variable classes that have - * fixed-sized Eigen member objects. + * @brief Convenience function that creates the required pointer aliases, clone() method, and type() method + * for derived Variable classes that have fixed-sized Eigen member objects. * * Usage: * @code{.cpp} @@ -147,36 +161,32 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ +#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_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) + namespace fuse_core { + /** * @brief The Variable interface definition. * - * A Variable defines some semantically meaningful group of one or more - * individual scale values. Each variable is treated as a block by the - * optimization engine, as the values of all of its dimensions are likely to be - * involved in the same constraints. Some common examples of variable groupings - * are a 2D point (x, y), 3D point (x, y, z), or camera calibration parameters - * (fx, fy, cx, cy). + * A Variable defines some semantically meaningful group of one or more individual scale values. Each variable is + * treated as a block by the optimization engine, as the values of all of its dimensions are likely to be involved + * in the same constraints. Some common examples of variable groupings are a 2D point (x, y), 3D point (x, y, z), or + * camera calibration parameters (fx, fy, cx, cy). * - * To support the Ceres optimization engine, the Variable must hold the scalar - * values of each dimension in a _contiguous_ memory space, and must provide - * access to that memory location via the Variable::data() methods. + * To support the Ceres optimization engine, the Variable must hold the scalar values of each dimension in a + * _contiguous_ memory space, and must provide access to that memory location via the Variable::data() methods. * - * Some Variables may require special update rules, either because they are - * over-parameterized, as is the case with 3D rotations represented as - * quaternions, or because the update of the individual dimensions exhibit some - * nonlinear properties, as is the case with rotations in general (e.g. 2D - * rotations have a discontinuity around π). To support these situations, - * Ceres uses an optional "local parameterization". See the Ceres documentation - * for more details. - * http://ceres-solver.org/nnls_modeling.html#localparameterization + * Some Variables may require special update rules, either because they are over-parameterized, as is the case with + * 3D rotations represented as quaternions, or because the update of the individual dimensions exhibit some nonlinear + * properties, as is the case with rotations in general (e.g. 2D rotations have a discontinuity around π). To + * support these situations, Ceres uses an optional "local parameterization". See the Ceres documentation for more + * details. http://ceres-solver.org/nnls_modeling.html#localparameterization */ class Variable { @@ -191,21 +201,16 @@ class Variable /** * @brief Constructor * - * The implemented UUID generation should be deterministic such that a - * variable with the same metadata will always return the same UUID. Identical - * UUIDs produced by sensors will be treated as the same variable by the - * optimizer, and different UUIDs will be treated as different variables. So, - * two derived variables representing robot poses with the same timestamp but - * different UUIDs will incorrectly be treated as different variables, and two - * robot poses with different timestamps but the same UUID will be incorrectly - * treated as the same variable. - * - * One method of producing UUIDs that adhere to this requirement is to use the - * boost::uuid::name_generator() function. The type() string can be used to - * generate a UUID namespace for all variables of a given derived type, and - * the variable metadata of consequence can be converted into a - * carefully-formatted string or byte array and provided to the generator to - * create the UUID for a specific variable instance. + * The implemented UUID generation should be deterministic such that a variable with the same metadata will always + * return the same UUID. Identical UUIDs produced by sensors will be treated as the same variable by the optimizer, + * and different UUIDs will be treated as different variables. So, two derived variables representing robot poses with + * the same timestamp but different UUIDs will incorrectly be treated as different variables, and two robot poses with + * different timestamps but the same UUID will be incorrectly treated as the same variable. + * + * One method of producing UUIDs that adhere to this requirement is to use the boost::uuid::name_generator() function. + * The type() string can be used to generate a UUID namespace for all variables of a given derived type, and the + * variable metadata of consequence can be converted into a carefully-formatted string or byte array and provided to + * the generator to create the UUID for a specific variable instance. * * @param[in] uuid The unique ID number for this variable */ @@ -224,47 +229,41 @@ class Variable /** * @brief Returns a unique name for this variable type. * - * The variable type string must be unique for each class. As such, the - * fully-qualified class name is an excellent choice for the type string. + * The variable type string must be unique for each class. As such, the fully-qualified class name is an excellent + * choice for the type string. * * The suggested implementation for all derived classes is: * @code{.cpp} - * return return - * boost::typeindex::stl_type_index::type_id().pretty_name(); + * return return boost::typeindex::stl_type_index::type_id().pretty_name(); * @endcode * - * To make this easy to implement in all derived classes, the - * FUSE_VARIABLE_TYPE_DEFINITION() and FUSE_VARIABLE_DEFINITIONS() macro - * functions have been provided. + * To make this easy to implement in all derived classes, the FUSE_VARIABLE_TYPE_DEFINITION() and + * FUSE_VARIABLE_DEFINITIONS() macro functions have been provided. */ virtual std::string type() const = 0; /** * @brief Returns the number of elements of this variable. * - * In most cases, this will be the number of degrees of freedom this variable - * represents. For example, a 2D pose has an x, y, and theta value, so the - * size will be 3. A notable exception is a 3D rotation represented as a - * quaternion. It only has 3 degrees of freedom, but it is represented as four - * elements, (w, x, y, z), so it's size will be 4. + * In most cases, this will be the number of degrees of freedom this variable represents. For example, a 2D pose has + * an x, y, and theta value, so the size will be 3. A notable exception is a 3D rotation represented as a quaternion. + * It only has 3 degrees of freedom, but it is represented as four elements, (w, x, y, z), so it's size will be 4. */ virtual size_t size() const = 0; /** * @brief Returns the number of elements of the local parameterization space. * - * If you override the \p localParameterization() method, it is good practice - * to also override the \p localSize() method. By default, the \p size() - * method is used for \p localSize() as well. + * If you override the \p localParameterization() method, it is good practice to also override the \p localSize() + * method. By default, the \p size() method is used for \p localSize() as well. */ virtual size_t localSize() const { return size(); } /** * @brief Read-only access to the variable data * - * The data elements must be contiguous (such as a C-style array double[3] or - * std::vector), and it must contain at least Variable::size() - * elements. Only Variable::size() elements will be accessed externally. This + * The data elements must be contiguous (such as a C-style array double[3] or std::vector), and it must + * contain at least Variable::size() elements. Only Variable::size() elements will be accessed externally. This * interface is provided for integration with Ceres, which uses raw pointers. */ virtual const double* data() const = 0; @@ -272,24 +271,21 @@ class Variable /** * @brief Read-write access to the variable data * - * The data elements must be contiguous (such as a C-style array double[3] or - * std::vector), and it must contain at least Variable::size() - * elements. Only Variable::size() elements will be accessed externally. This + * The data elements must be contiguous (such as a C-style array double[3] or std::vector), and it must + * contain at least Variable::size() elements. Only Variable::size() elements will be accessed externally. This * interface is provided for integration with Ceres, which uses raw pointers. */ virtual double* data() = 0; /** - * @brief Print a human-readable description of the variable to the provided - * stream. + * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ virtual void print(std::ostream& stream = std::cout) const = 0; /** - * @brief Perform a deep copy of the Variable and return a unique pointer to - * the copy + * @brief Perform a deep copy of the Variable and return a unique pointer to the copy * * Unique pointers can be implicitly upgraded to shared pointers if needed. * @@ -298,33 +294,31 @@ class Variable * return Derived::make_unique(*this); * @endcode * - * To make this easy to implement in all derived classes, the - * FUSE_VARIABLE_CLONE_DEFINITION() and FUSE_VARIABLE_DEFINITIONS() macros - * functions have been provided. + * To make this easy to implement in all derived classes, the FUSE_VARIABLE_CLONE_DEFINITION() and + * FUSE_VARIABLE_DEFINITIONS() macros functions have been provided. * * @return A unique pointer to a new instance of the most-derived Variable */ virtual Variable::UniquePtr clone() const = 0; /** - * @brief Create a new Ceres local parameterization object to apply to updates - * of this variable + * @brief Create a new Ceres local parameterization object to apply to updates of this variable * - * If a local parameterization 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 + * If a local parameterization 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. + * 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 LocalParameterization */ - virtual fuse_core::LocalParameterization* localParameterization() const { return nullptr; } + virtual fuse_core::LocalParameterization* localParameterization() const + { + return nullptr; + } #if CERES_SUPPORTS_MANIFOLDS /** @@ -346,6 +340,10 @@ class Variable */ 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) { @@ -366,7 +364,10 @@ class Variable * @param[in] index The variable dimension of interest * @return The lower bound for the requested variable dimension */ - virtual double lowerBound(size_t index) const { return std::numeric_limits::lowest(); } + virtual double lowerBound(size_t index) const + { + return std::numeric_limits::lowest(); + } /** * @brief Specifies the upper bound value of each variable dimension @@ -376,13 +377,18 @@ class Variable * @param[in] index The variable dimension of interest * @return The upper bound for the requested variable dimension */ - virtual double upperBound(size_t index) const { return std::numeric_limits::max(); } + virtual double upperBound(size_t index) const + { + return std::numeric_limits::max(); + } /** - * @brief Specifies if the value of the variable should not be changed during - * optimization + * @brief Specifies if the value of the variable should not be changed during optimization */ - virtual bool holdConstant() const { return false; } + virtual bool holdConstant() const + { + return false; + } /** * @brief Serialize this Variable into the provided binary archive @@ -439,30 +445,26 @@ class Variable 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/out of the archive * - * This method, or a combination of save() and load() methods, must be - * implemented by all derived classes. See documentation on Boost - * Serialization for information on how to implement the serialize() method. + * This method, or a combination of save() and load() methods, must be implemented by all derived classes. See + * documentation on Boost Serialization for information on how to implement the serialize() method. * https://www.boost.org/doc/libs/1_70_0/libs/serialization/doc/ * - * @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[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 + template void serialize(Archive& archive, const unsigned int /* version */) { - archive& uuid_; + archive & uuid_; } }; /** * Stream operator implementation used for all derived Variable classes. */ -std::ostream& operator<<(std::ostream& stream, const Variable& variable); +std::ostream& operator <<(std::ostream& stream, const Variable& variable); } // namespace fuse_core diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 99acbcc50..00fb6c80b 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -147,7 +147,9 @@ void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& nh.param("use_explicit_schur_complement", solver_options.use_explicit_schur_complement, solver_options.use_explicit_schur_complement); +#if !CERES_VERSION_AT_LEAST(2, 2, 0) nh.param("use_postordering", solver_options.use_postordering, solver_options.use_postordering); +#endif nh.param("dynamic_sparsity", solver_options.dynamic_sparsity, solver_options.dynamic_sparsity); #if CERES_VERSION_AT_LEAST(2, 0, 0) @@ -157,6 +159,15 @@ void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& solver_options.max_num_refinement_iterations); #endif +#if CERES_VERSION_AT_LEAST(2, 2, 0) + nh.param("max_num_spse_iterations", solver_options.max_num_spse_iterations, + solver_options.max_num_spse_iterations); + nh.param("use_spse_initialization", solver_options.use_spse_initialization, + solver_options.use_spse_initialization); + nh.param("spse_tolerance", solver_options.spse_tolerance, + solver_options.spse_tolerance); +#endif + nh.param("use_inner_iterations", solver_options.use_inner_iterations, solver_options.use_inner_iterations); // No parameter is loaded for: std::shared_ptr inner_iteration_ordering; diff --git a/fuse_core/src/manifold_adapter.cpp b/fuse_core/src/manifold_adapter.cpp new file mode 100644 index 000000000..385de515f --- /dev/null +++ b/fuse_core/src/manifold_adapter.cpp @@ -0,0 +1,40 @@ +/* + * 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 + +#if CERES_SUPPORTS_MANIFOLDS +#include + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_core::ManifoldAdapter); +#endif diff --git a/fuse_core/test/example_variable.h b/fuse_core/test/example_variable.h index 10600afb2..2e7987ee8 100644 --- a/fuse_core/test/example_variable.h +++ b/fuse_core/test/example_variable.h @@ -42,6 +42,7 @@ #include #include #include +#include /** @@ -83,6 +84,138 @@ 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; + 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 + 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; + 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 + 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} 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 d49c19182..cbd146e96 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include - #include #include @@ -43,7 +42,7 @@ struct Plus { - template + template bool operator()(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = x[0] + 2.0 * delta[0]; @@ -55,7 +54,7 @@ struct Plus struct Minus { - template + template bool operator()(const T* x, const T* y, T* y_minus_x) const { y_minus_x[0] = (y[0] - x[0]) / 2.0; @@ -66,13 +65,14 @@ struct Minus using TestLocalParameterization = fuse_core::AutoDiffLocalParameterization; + TEST(LocalParameterization, Plus) { TestLocalParameterization parameterization; - double x[3] = { 1.0, 2.0, 3.0 }; - double delta[2] = { 0.5, 1.0 }; - double actual[3] = { 0.0, 0.0, 0.0 }; + double x[3] = {1.0, 2.0, 3.0}; + double delta[2] = {0.5, 1.0}; + double actual[3] = {0.0, 0.0, 0.0}; bool success = parameterization.Plus(x, delta, actual); EXPECT_TRUE(success); @@ -85,12 +85,14 @@ TEST(LocalParameterization, PlusJacobian) { TestLocalParameterization parameterization; - double x[3] = { 1.0, 2.0, 3.0 }; + double x[3] = {1.0, 2.0, 3.0}; fuse_core::MatrixXd actual(3, 2); bool success = parameterization.ComputeJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 2); - expected << 2.0, 0.0, 0.0, 5.0, 0.0, 0.0; + expected << 2.0, 0.0, + 0.0, 5.0, + 0.0, 0.0; EXPECT_TRUE(success); EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5); @@ -100,9 +102,9 @@ TEST(LocalParameterization, Minus) { TestLocalParameterization parameterization; - double x1[3] = { 1.0, 2.0, 3.0 }; - double x2[3] = { 2.0, 7.0, 3.0 }; - double actual[2] = { 0.0, 0.0 }; + double x1[3] = {1.0, 2.0, 3.0}; + double x2[3] = {2.0, 7.0, 3.0}; + double actual[2] = {0.0, 0.0}; bool success = parameterization.Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -114,12 +116,13 @@ TEST(LocalParameterization, MinusJacobian) { TestLocalParameterization parameterization; - double x[3] = { 1.0, 2.0, 3.0 }; + double x[3] = {1.0, 2.0, 3.0}; fuse_core::MatrixXd actual(2, 3); bool success = parameterization.ComputeMinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(2, 3); - expected << 0.5, 0.0, 0.0, 0.0, 0.2, 0.0; + expected << 0.5, 0.0, 0.0, + 0.0, 0.2, 0.0; EXPECT_TRUE(success); EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5); @@ -156,7 +159,6 @@ TEST(LocalParameterization, PlusMinus) EXPECT_NEAR(delta[0], actual[0], 1.0e-5); EXPECT_NEAR(delta[1], actual[1], 1.0e-5); } - #endif int main(int argc, char** argv) diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index cf09540b1..59096878a 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -33,8 +33,15 @@ */ #include +#include +#include +#include +#include +#include #include +#include +#include TEST(Variable, Type) { @@ -42,6 +49,158 @@ TEST(Variable, Type) 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; + 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 + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.h b/fuse_graphs/include/fuse_graphs/hash_graph.h index 25baee2a3..56f358631 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph.h +++ b/fuse_graphs/include/fuse_graphs/hash_graph.h @@ -424,14 +424,14 @@ void serialize(Archive& archive, ceres::Problem::Options& options, const unsigne archive & options.cost_function_ownership; archive & options.disable_all_safety_checks; archive & options.enable_fast_removal; -#if CERES_SUPPORTS_MANIFOLDS +#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; -#else - archive & options.local_parameterization_ownership; #endif archive & options.loss_function_ownership; } diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index b5d421cbc..5b31cf4b3 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -481,7 +481,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) { 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 4df3dc620..e7b1a9afd 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.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 diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h index 0e04e746b..36979dcef 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_ACCELERATION_ANGULAR_2D_STAMPED_H #define FUSE_VARIABLES_ACCELERATION_ANGULAR_2D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -102,6 +104,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.h b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h index 097b18d7e..ce333441e 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_ACCELERATION_ANGULAR_3D_STAMPED_H #define FUSE_VARIABLES_ACCELERATION_ANGULAR_3D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -125,6 +127,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.h b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h index a8c7156c1..06a88876b 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_ACCELERATION_LINEAR_2D_STAMPED_H #define FUSE_VARIABLES_ACCELERATION_LINEAR_2D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -113,6 +115,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.h b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h index 631622ad2..2efe82109 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_ACCELERATION_LINEAR_3D_STAMPED_H #define FUSE_VARIABLES_ACCELERATION_LINEAR_3D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -125,6 +127,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.h b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h index fa278f002..f7be722a0 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h @@ -34,7 +34,10 @@ #ifndef FUSE_VARIABLES_ORIENTATION_2D_STAMPED_H #define FUSE_VARIABLES_ORIENTATION_2D_STAMPED_H +#include +#include #include +#include #include #include #include @@ -49,8 +52,10 @@ #include + namespace fuse_variables { + /** * @brief A LocalParameterization class for 2D Orientations. * @@ -61,31 +66,49 @@ namespace fuse_variables class Orientation2DLocalParameterization : public fuse_core::LocalParameterization { public: - int GlobalSize() const override { return 1; } + FUSE_SMART_PTR_DEFINITIONS(Orientation2DLocalParameterization); + + int GlobalSize() const override + { + return 1; + } - int LocalSize() const override { return 1; } + int LocalSize() const override + { + return 1; + } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + 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 ComputeJacobian(const double* /*x*/, double* jacobian) const override + bool ComputeJacobian( + const double* /*x*/, + double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus(const double* x, const double* y, double* y_minus_x) const override + bool Minus( + const double* x, + const double* y, + double* y_minus_x) const override { - // Compute the difference from y to x, and handle the 2*Pi rollover + // 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; } - bool ComputeMinusJacobian(const double* /*x*/, double* jacobian) const override + bool ComputeMinusJacobian( + const double* /*x*/, + double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -101,14 +124,13 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati * @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 + template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object(*this); + archive & boost::serialization::base_object(*this); } }; - #if CERES_SUPPORTS_MANIFOLDS /** * @brief A Manifold class for 2D Orientations. @@ -120,6 +142,8 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati class Orientation2DManifold : public fuse_core::Manifold { public: + FUSE_SMART_PTR_DEFINITIONS(Orientation2DManifold); + int AmbientSize() const override { return 1; } int TangentSize() const override { return 1; } @@ -163,10 +187,9 @@ class Orientation2DManifold : public fuse_core::Manifold template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object(*this); + archive & boost::serialization::base_object(*this); } }; - #endif /** @@ -283,7 +306,6 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped #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.h b/fuse_variables/include/fuse_variables/orientation_3d_stamped.h index 0132b6ca7..221eab6de 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.h @@ -34,7 +34,10 @@ #ifndef FUSE_VARIABLES_ORIENTATION_3D_STAMPED_H #define FUSE_VARIABLES_ORIENTATION_3D_STAMPED_H +#include +#include #include +#include #include #include #include @@ -50,6 +53,7 @@ #include + namespace fuse_variables { /** @@ -76,11 +80,22 @@ inline static void QuaternionInverse(const T in[4], T out[4]) class Orientation3DLocalParameterization : public fuse_core::LocalParameterization { public: - int GlobalSize() const override { return 4; } + FUSE_SMART_PTR_DEFINITIONS(Orientation3DLocalParameterization); - int LocalSize() const override { return 3; } + int GlobalSize() const override + { + return 4; + } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + 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); @@ -88,7 +103,9 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeJacobian(const double* x, double* jacobian) const override + bool ComputeJacobian( + const double* x, + double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; @@ -101,7 +118,10 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool Minus(const double* x, const double* y, double* y_minus_x) const override + bool Minus( + const double* x, + const double* y, + double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -111,7 +131,9 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeMinusJacobian(const double* x, double* jacobian) const override + bool ComputeMinusJacobian( + const double* x, + double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; @@ -151,6 +173,8 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati class Orientation3DManifold : public fuse_core::Manifold { public: + FUSE_SMART_PTR_DEFINITIONS(Orientation3DManifold); + int AmbientSize() const override { return 4; } int TangentSize() const override { return 3; } @@ -214,7 +238,6 @@ class Orientation3DManifold : public fuse_core::Manifold archive & boost::serialization::base_object(*this); } }; - #endif /** @@ -376,7 +399,6 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped #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/pinhole_camera.h b/fuse_variables/include/fuse_variables/pinhole_camera.h index c967cdff1..2d4a089a1 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.h +++ b/fuse_variables/include/fuse_variables/pinhole_camera.h @@ -37,7 +37,9 @@ #ifndef FUSE_VARIABLES_PINHOLE_CAMERA_H #define FUSE_VARIABLES_PINHOLE_CAMERA_H +#include #include +#include #include #include #include @@ -147,6 +149,15 @@ class PinholeCamera : public FixedSizeVariable<4> */ 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 + protected: /** * @brief Construct a point 3D variable given a landmarks id diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h index abc0c388a..f1397e97d 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h @@ -37,7 +37,9 @@ #ifndef FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H #define FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H +#include #include +#include #include #include @@ -81,6 +83,15 @@ class PinholeCameraFixed : public PinholeCamera */ bool holdConstant() const override { return true; } +#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_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h index 4d5007512..daccfdab0 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h @@ -34,7 +34,9 @@ #ifndef FUSE_VARIABLES_POINT_2D_FIXED_LANDMARK_H #define FUSE_VARIABLES_POINT_2D_FIXED_LANDMARK_H +#include #include +#include #include #include @@ -74,6 +76,15 @@ class Point2DFixedLandmark : public Point2DLandmark */ bool holdConstant() const override { return true; } +#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.h b/fuse_variables/include/fuse_variables/point_2d_landmark.h index 1ebd80d50..b5c39fe6a 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.h @@ -34,7 +34,9 @@ #ifndef FUSE_VARIABLES_POINT_2D_LANDMARK_H #define FUSE_VARIABLES_POINT_2D_LANDMARK_H +#include #include +#include #include #include #include @@ -113,6 +115,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 + protected: /** * @brief Construct a point 2D variable given a UUID and a landmarks id diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h index d690bf9ed..74b93391d 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h @@ -34,7 +34,9 @@ #ifndef FUSE_VARIABLES_POINT_3D_FIXED_LANDMARK_H #define FUSE_VARIABLES_POINT_3D_FIXED_LANDMARK_H +#include #include +#include #include #include @@ -74,6 +76,15 @@ class Point3DFixedLandmark : public Point3DLandmark */ bool holdConstant() const override { return true; } +#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.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h index c165a68d3..501d5c57d 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.h @@ -37,7 +37,9 @@ #ifndef FUSE_VARIABLES_POINT_3D_LANDMARK_H #define FUSE_VARIABLES_POINT_3D_LANDMARK_H +#include #include +#include #include #include #include @@ -127,6 +129,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 + protected: /** * @brief Construct a point 3D variable given a landmarks id diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.h b/fuse_variables/include/fuse_variables/position_2d_stamped.h index bcd4916d2..e42d4fc9e 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_POSITION_2D_STAMPED_H #define FUSE_VARIABLES_POSITION_2D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -112,6 +114,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.h b/fuse_variables/include/fuse_variables/position_3d_stamped.h index 5991bd81b..9bc8e76dc 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_POSITION_3D_STAMPED_H #define FUSE_VARIABLES_POSITION_3D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -124,6 +126,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.h b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h index ba90a2458..6c3ca03ef 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_VELOCITY_ANGULAR_2D_STAMPED_H #define FUSE_VARIABLES_VELOCITY_ANGULAR_2D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -100,6 +102,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.h b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h index d004abeae..ed443ce30 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_VELOCITY_ANGULAR_3D_STAMPED_H #define FUSE_VARIABLES_VELOCITY_ANGULAR_3D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -123,6 +125,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.h b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h index 123bfa134..b2f5f3eb8 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_VELOCITY_LINEAR_2D_STAMPED_H #define FUSE_VARIABLES_VELOCITY_LINEAR_2D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -112,6 +114,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.h b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h index ad4824066..a555947f7 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h @@ -34,8 +34,10 @@ #ifndef FUSE_VARIABLES_VELOCITY_LINEAR_3D_STAMPED_H #define FUSE_VARIABLES_VELOCITY_LINEAR_3D_STAMPED_H -#include +#include +#include #include +#include #include #include #include @@ -124,6 +126,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 ee11938bd..0c992f376 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -82,7 +83,6 @@ fuse_core::Manifold* Orientation2DStamped::manifold() const #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 9f48e79a9..37ae7d3d0 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -34,6 +34,7 @@ #include #include +#include #include #include #include diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index c4bdbbf46..a9a931dbc 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -33,11 +33,7 @@ */ #include #include -#if CERES_SUPPORTS_MANIFOLDS -#include -#else #include -#endif #include #include #include @@ -52,6 +48,7 @@ #include #include + using fuse_variables::Orientation2DStamped; TEST(Orientation2DStamped, Type) @@ -93,8 +90,8 @@ TEST(Orientation2DStamped, UUID) TEST(Orientation2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = - Orientation2DStamped::make_shared(ros::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = Orientation2DStamped::make_shared(ros::Time(12345678, 910111213), + fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(ros::Time(12345678, 910111213), derived->stamp()); @@ -106,27 +103,9 @@ TEST(Orientation2DStamped, Stamped) EXPECT_EQ(fuse_core::uuid::generate("mo"), stamped->deviceId()); } -#if CERES_SUPPORTS_MANIFOLDS -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; - } -}; -#else struct Orientation2DPlus { - template + template bool operator()(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); @@ -143,14 +122,9 @@ struct Orientation2DMinus return true; } }; -#endif -using Orientation2DAutoDiff = -#if CERES_SUPPORTS_MANIFOLDS - ceres::AutoDiffManifold; -#else - fuse_core::AutoDiffLocalParameterization; -#endif +using Orientation2DLocalParameterization = + fuse_core::AutoDiffLocalParameterization; TEST(Orientation2DStamped, Plus) { @@ -158,9 +132,9 @@ TEST(Orientation2DStamped, Plus) // Simple test { - double x[1] = { 1.0 }; - double delta[1] = { 0.5 }; - double actual[1] = { 0.0 }; + double x[1] = {1.0}; + double delta[1] = {0.5}; + double actual[1] = {0.0}; bool success = parameterization->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -169,13 +143,13 @@ TEST(Orientation2DStamped, Plus) // Check roll-over { - double x[1] = { 2.0 }; - double delta[1] = { 3.0 }; - double actual[1] = { 0.0 }; + double x[1] = {2.0}; + double delta[1] = {3.0}; + double actual[1] = {0.0}; bool success = parameterization->Plus(x, delta, actual); EXPECT_TRUE(success); - EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); + EXPECT_NEAR(5 - 2*M_PI, actual[0], 1.0e-5); } delete parameterization; @@ -183,30 +157,18 @@ TEST(Orientation2DStamped, Plus) TEST(Orientation2DStamped, PlusJacobian) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation2DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation2DStamped(ros::Time(0, 0)).manifold(); -#endif - auto reference = Orientation2DAutoDiff(); + auto reference = Orientation2DLocalParameterization(); - auto test_values = std::vector { -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + 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 }; -#if !CERES_SUPPORTS_MANIFOLDS + double x[1] = {test_value}; + double actual[1] = {0.0}; bool success = parameterization->ComputeJacobian(x, actual); -#else - bool success = parameterization->PlusJacobian(x, actual); -#endif - double expected[1] = { 0.0 }; -#if !CERES_SUPPORTS_MANIFOLDS + double expected[1] = {0.0}; reference.ComputeJacobian(x, expected); -#else - reference.PlusJacobian(x, expected); -#endif EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); @@ -217,22 +179,14 @@ TEST(Orientation2DStamped, PlusJacobian) TEST(Orientation2DStamped, Minus) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation2DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation2DStamped(ros::Time(0, 0)).manifold(); -#endif // Simple test { - double x1[1] = { 1.0 }; - double x2[1] = { 1.5 }; - double actual[1] = { 0.0 }; -#if !CERES_SUPPORTS_MANIFOLDS + double x1[1] = {1.0}; + double x2[1] = {1.5}; + double actual[1] = {0.0}; bool success = parameterization->Minus(x1, x2, actual); -#else - bool success = parameterization->Minus(x2, x1, actual); -#endif EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -240,14 +194,10 @@ TEST(Orientation2DStamped, Minus) // Check roll-over { - double x1[1] = { 2.0 }; - double x2[1] = { 5 - 2 * M_PI }; - double actual[1] = { 0.0 }; -#if !CERES_SUPPORTS_MANIFOLDS + double x1[1] = {2.0}; + double x2[1] = {5 - 2*M_PI}; + double actual[1] = {0.0}; bool success = parameterization->Minus(x1, x2, actual); -#else - bool success = parameterization->Minus(x2, x1, actual); -#endif EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -256,30 +206,18 @@ TEST(Orientation2DStamped, Minus) TEST(Orientation2DStamped, MinusJacobian) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation2DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation2DStamped(ros::Time(0, 0)).manifold(); -#endif - auto reference = Orientation2DAutoDiff(); + auto reference = Orientation2DLocalParameterization(); - auto test_values = std::vector { -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + 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 }; -#if !CERES_SUPPORTS_MANIFOLDS + double x[1] = {test_value}; + double actual[1] = {0.0}; bool success = parameterization->ComputeMinusJacobian(x, actual); -#else - bool success = parameterization->MinusJacobian(x, actual); -#endif - double expected[1] = { 0.0 }; -#if !CERES_SUPPORTS_MANIFOLDS + double expected[1] = {0.0}; reference.ComputeMinusJacobian(x, expected); -#else - reference.MinusJacobian(x, expected); -#endif EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); @@ -292,8 +230,7 @@ struct CostFunctor { CostFunctor() {} - template - bool operator()(const T* const x, T* residual) const + template bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); return true; @@ -312,13 +249,22 @@ TEST(Orientation2DStamped, Optimization) // Build the problem. ceres::Problem problem; #if !CERES_SUPPORTS_MANIFOLDS - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); + problem.AddParameterBlock( + orientation.data(), + orientation.size(), + orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + 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); + problem.AddResidualBlock( + cost_function, + nullptr, + parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -355,6 +301,129 @@ TEST(Orientation2DStamped, Serialization) EXPECT_EQ(expected.getYaw(), actual.getYaw()); } +#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(ros::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(ros::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(ros::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(ros::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 + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 9ba7b8f73..6a95aa583 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -33,11 +33,7 @@ */ #include #include -#if CERES_SUPPORTS_MANIFOLDS -#include -#else #include -#endif #include #include #include @@ -121,32 +117,9 @@ inline static void QuaternionInverse(const T in[4], T out[4]) out[3] = -in[3]; } -#if CERES_SUPPORTS_MANIFOLDS -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; - } -}; -#else struct Orientation3DPlus { - template + template bool operator()(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; @@ -169,22 +142,13 @@ struct Orientation3DMinus return true; } }; -#endif -using Orientation3DAutoDiff = -#if CERES_SUPPORTS_MANIFOLDS -ceres::AutoDiffManifold; -#else -fuse_core::AutoDiffLocalParameterization; -#endif +using Orientation3DLocalParameterization = + fuse_core::AutoDiffLocalParameterization; TEST(Orientation3DStamped, Plus) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation3DStamped(ros::Time(0, 0)).manifold(); -#endif double x[4] = {0.842614977, 0.2, 0.3, 0.4}; double delta[3] = {0.15, -0.2, 0.433012702}; @@ -202,17 +166,12 @@ TEST(Orientation3DStamped, Plus) TEST(Orientation3DStamped, Minus) { + auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization(); + 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}; - -#if !CERES_SUPPORTS_MANIFOLDS - auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization(); bool success = parameterization->Minus(x1, x2, result); -#else - auto parameterization = Orientation3DStamped(ros::Time(0, 0)).manifold(); - bool success = parameterization->Minus(x2, x1, result); -#endif EXPECT_TRUE(success); EXPECT_NEAR(0.15, result[0], 1.0e-5); @@ -224,12 +183,8 @@ TEST(Orientation3DStamped, Minus) TEST(Orientation3DStamped, PlusJacobian) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation3DStamped(ros::Time(0, 0)).manifold(); -#endif - auto reference = Orientation3DAutoDiff(); + auto reference = Orientation3DLocalParameterization(); for (double qx = -0.5; qx < 0.5; qx += 0.1) { @@ -245,22 +200,14 @@ TEST(Orientation3DStamped, PlusJacobian) 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; -#if !CERES_SUPPORTS_MANIFOLDS bool success = parameterization->ComputeJacobian(x, actual.data()); -#else - bool success = parameterization->PlusJacobian(x, actual.data()); -#endif fuse_core::MatrixXd expected(4, 3); 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; -#if !CERES_SUPPORTS_MANIFOLDS reference.ComputeJacobian(x, expected.data()); -#else - reference.PlusJacobian(x, expected.data()); -#endif EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); @@ -277,12 +224,8 @@ TEST(Orientation3DStamped, PlusJacobian) TEST(Orientation3DStamped, MinusJacobian) { -#if !CERES_SUPPORTS_MANIFOLDS auto parameterization = Orientation3DStamped(ros::Time(0, 0)).localParameterization(); -#else - auto parameterization = Orientation3DStamped(ros::Time(0, 0)).manifold(); -#endif - auto reference = Orientation3DAutoDiff(); + auto reference = Orientation3DLocalParameterization(); for (double qx = -0.5; qx < 0.5; qx += 0.1) { @@ -297,21 +240,13 @@ TEST(Orientation3DStamped, MinusJacobian) 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; -#if !CERES_SUPPORTS_MANIFOLDS bool success = parameterization->ComputeMinusJacobian(x, actual.data()); -#else - bool success = parameterization->MinusJacobian(x, actual.data()); -#endif fuse_core::MatrixXd expected(3, 4); 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; -#if !CERES_SUPPORTS_MANIFOLDS reference.ComputeMinusJacobian(x, expected.data()); -#else - reference.MinusJacobian(x, expected.data()); -#endif EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); @@ -493,6 +428,182 @@ TEST(Orientation3DStamped, Serialization) 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(ros::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(ros::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); + 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; + bool success = manifold->PlusJacobian(x, actual.data()); + + fuse_core::MatrixXd expected(4, 3); + 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; + 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(ros::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(ros::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); + 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; + bool success = manifold->MinusJacobian(x, actual.data()); + + fuse_core::MatrixXd expected(3, 4); + 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; + 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(ros::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 + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);