diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 5b8b604e4..416508d7a 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -18,7 +18,7 @@ jobs: - uses: actions/setup-python@v5 with: python-version: '3.10' - - name: Install clang-format-14 + - name: Install clang-format-18 run: sudo apt-get install clang-format-18 - uses: pre-commit/action@v3.0.1 id: precommit diff --git a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp index 14c1b7d2d..7e489ef1b 100644 --- a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp @@ -43,11 +43,10 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture { public: - NormalDeltaPose2DBenchmarkFixture() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + NormalDeltaPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -58,18 +57,18 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function covariance @@ -88,69 +87,56 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture }; // Cost function covariance -const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2}; +const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance = - fuse_core::Vector3d(covariance_diagonal).asDiagonal(); + fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalDeltaPose2DBenchmarkFixture::position1[] = {0.0, 1.0}; -const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = {0.5}; -const double NormalDeltaPose2DBenchmarkFixture::position2[] = {2.0, 3.0}; -const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = {1.5}; +const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 }; +const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 }; +const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 }; +const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 }; // Delta and sqrt information matrix -const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{1.0, 2.0, 3.0}; -const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse(). - llt().matrixU()); +const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 }; +const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double * NormalDeltaPose2DBenchmarkFixture::parameters[] = -{position1, orientation1, position2, orientation2}; +const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 }; -const std::vector & NormalDeltaPose2DBenchmarkFixture::block_sizes = {2, 1, 2, 1}; +const std::vector& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 }; const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3; -BENCHMARK_DEFINE_F( - NormalDeltaPose2DBenchmarkFixture, - AnalyticNormalDeltaPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)(benchmark::State& state) { // Create analytic cost function - const fuse_constraints::NormalDeltaPose2D cost_function{sqrt_information.topRows(state.range(0)), - delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ sqrt_information.topRows(state.range(0)), delta }; - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange(1, 3); -BENCHMARK_DEFINE_F( - NormalDeltaPose2DBenchmarkFixture, - AutoDiffNormalDeltaPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); - const ceres::AutoDiffCostFunction< - fuse_constraints::NormalDeltaPose2DCostFunctor, - ceres::DYNAMIC, - 2, 1, 2, 1 - > - cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor( - partial_sqrt_information, - delta), - partial_sqrt_information.rows()); - - for (auto _ : state) { + const ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta), + partial_sqrt_information.rows()); + + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange(1, 3); BENCHMARK_MAIN(); diff --git a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp index f1808f942..c550a82cf 100644 --- a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp @@ -43,11 +43,10 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture { public: - NormalPriorPose2DBenchmarkFixture() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + NormalPriorPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -58,18 +57,18 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function covariance @@ -86,66 +85,54 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture }; // Cost function covariance -const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2}; +const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance = - fuse_core::Vector3d(covariance_diagonal).asDiagonal(); + fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalPriorPose2DBenchmarkFixture::position[] = {0.0, 0.0}; -const double NormalPriorPose2DBenchmarkFixture::orientation[] = {0.0}; +const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 }; +const double NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 }; // Mean and sqrt information matrix -const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{1.0, 2.0, 3.0}; -const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse(). - llt().matrixU()); +const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 }; +const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double * NormalPriorPose2DBenchmarkFixture::parameters[] = {position, orientation}; +const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation }; -const std::vector & NormalPriorPose2DBenchmarkFixture::block_sizes = {2, 1}; +const std::vector& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 }; const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3; -BENCHMARK_DEFINE_F( - NormalPriorPose2DBenchmarkFixture, - AnalyticNormalPriorPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)(benchmark::State& state) { // Create analytic cost function - const fuse_constraints::NormalPriorPose2D cost_function{sqrt_information.topRows(state.range(0)), - mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ sqrt_information.topRows(state.range(0)), mean }; - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange(1, 3); -BENCHMARK_DEFINE_F( - NormalPriorPose2DBenchmarkFixture, - AutoDiffNormalPriorPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); - const ceres::AutoDiffCostFunction< - fuse_constraints::NormalPriorPose2DCostFunctor, - ceres::DYNAMIC, - 2, 1 - > - cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor( - partial_sqrt_information, - mean), - partial_sqrt_information.rows()); - - for (auto _ : state) { + const ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean), + partial_sqrt_information.rows()); + + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange(1, 3); BENCHMARK_MAIN(); diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..0e02561bd 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -57,7 +57,6 @@ #include #include - namespace fuse_constraints { @@ -71,7 +70,7 @@ namespace fuse_constraints * prior map (scan-to-map measurements). This constraint holds the measured variable value and the * measurement uncertainty/covariance. */ -template +template class AbsoluteConstraint : public fuse_core::Constraint { public: @@ -90,11 +89,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior value of all variable dimensions * @param[in] covariance The measurement/prior uncertainty of all variable dimensions */ - AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance); + AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& mean, + const fuse_core::MatrixXd& covariance); /** * @brief Create a constraint using a measurement/prior of only a partial set of dimensions of the @@ -109,12 +105,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * by \p indices. * @param[in] indices The set of indices corresponding to the measured dimensions */ - AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices); + AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& indices); /** * @brief Destructor @@ -128,7 +120,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd & mean() const {return mean_;} + const fuse_core::VectorXd& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -138,7 +133,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -155,7 +153,7 @@ class AbsoluteConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -167,10 +165,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable + fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -184,28 +182,23 @@ class AbsoluteConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; // Define unique names for the different variations of the absolute constraint -using AbsoluteAccelerationAngular2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteAccelerationLinear2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteOrientation2DStampedConstraint = - AbsoluteConstraint; +using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; -using AbsoluteVelocityAngular2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteVelocityLinear2DStampedConstraint = - AbsoluteConstraint; +using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..f28ceb3bf 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -42,33 +42,28 @@ #include - namespace fuse_constraints { -template -AbsoluteConstraint::AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance) -: fuse_core::Constraint(source, {variable.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) +template +AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, + const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance) + : fuse_core::Constraint(source, { variable.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { assert(mean.rows() == static_cast(variable.size())); assert(covariance.rows() == static_cast(variable.size())); assert(covariance.cols() == static_cast(variable.size())); } -template -AbsoluteConstraint::AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices) -: fuse_core::Constraint(source, {variable.uuid()}) // NOLINT(whitespace/braces) +template +AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, + const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& indices) + : fuse_core::Constraint(source, { variable.uuid() }) // NOLINT(whitespace/braces) { assert(partial_mean.rows() == static_cast(indices.size())); assert(partial_covariance.rows() == static_cast(indices.size())); @@ -86,13 +81,14 @@ AbsoluteConstraint::AbsoluteConstraint( // and the columns are in the order defined by the variable. mean_ = fuse_core::VectorXd::Zero(variable.size()); sqrt_information_ = fuse_core::MatrixXd::Zero(indices.size(), variable.size()); - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { mean_(indices[i]) = partial_mean(i); sqrt_information_.col(indices[i]) = partial_sqrt_information.col(i); } } -template +template fuse_core::MatrixXd AbsoluteConstraint::covariance() const { // We want to compute: @@ -107,8 +103,8 @@ fuse_core::MatrixXd AbsoluteConstraint::covariance() const return pinv * pinv.transpose(); } -template -void AbsoluteConstraint::print(std::ostream & stream) const +template +void AbsoluteConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -117,14 +113,15 @@ void AbsoluteConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -template -ceres::CostFunction * AbsoluteConstraint::costFunction() const +template +ceres::CostFunction* AbsoluteConstraint::costFunction() const { // Ceres ships with a "prior" cost function. Just use that here. return new ceres::NormalPrior(sqrt_information_, mean_); @@ -133,53 +130,50 @@ ceres::CostFunction * AbsoluteConstraint::costFunction() const // Specialization for Orientation2D // We need to handle the 2*pi rollover for 2D orientations, so simple subtraction does not produce // the correct cost -template<> -inline ceres::CostFunction * AbsoluteConstraint< - fuse_variables::Orientation2DStamped ->::costFunction() -const +template <> +inline ceres::CostFunction* AbsoluteConstraint::costFunction() const { return new NormalPriorOrientation2D(sqrt_information_(0, 0), mean_(0)); } // Specialize the type() method to return the name that is registered with the plugins -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteOrientation2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsolutePosition2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsolutePosition3DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h index e02d054f3..92eb503a2 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp index 0f63bdf86..d3f7b02c0 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -80,11 +79,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a quaternion (4x1 vector: w, x, y, z) * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::Matrix3d & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector4d& mean, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -94,11 +91,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as an Eigen quaternion * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const Eigen::Quaterniond & mean, - const fuse_core::Matrix3d & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const Eigen::Quaterniond& mean, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -108,11 +103,10 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a ROS quaternion message * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const geometry_msgs::msg::Quaternion & mean, - const std::array & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const geometry_msgs::msg::Quaternion& mean, + const std::array& covariance); /** * @brief Destructor @@ -124,14 +118,20 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d & mean() const {return mean_;} + const fuse_core::Vector4d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z) */ - const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix3d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -145,7 +145,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -157,7 +157,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: /** @@ -165,23 +165,23 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion - The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond & quaternion); + static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d * @param[in] quaternion - The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion & quaternion); + static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix * @param[in] covariance - The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array & covariance); + static fuse_core::Matrix3d toEigen(const std::array& covariance); - fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix private: @@ -195,12 +195,12 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h index 4d388fa17..0511e627f 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp index b224a2878..bcf527abc 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -87,12 +86,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] axes Used to specify which of the Euler axes they want to include in the * constraint, e.g. "{ Euler::ROLL, EULER::YAW }" */ - AbsoluteOrientation3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & axes); + AbsoluteOrientation3DStampedEulerConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance, + const std::vector& axes); /** * @brief Destructor @@ -103,7 +100,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @brief Read-only access to the vector that dictates the order of the Euler axes in the \p mean, * \p covariance, and \p sqrtInformation. */ - const std::vector axes() const {return axes_;} + const std::vector axes() const + { + return axes_; + } /** * @brief Read-only access to the measured/prior vector of mean values. @@ -112,7 +112,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * other currently implemented constraints in that the order does _not_ match the order defined in * the variable. */ - const fuse_core::VectorXd & mean() const {return mean_;} + const fuse_core::VectorXd& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -121,7 +124,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * from all other currently implemented constraints in that the order does _not_ match the order * defined in the variable. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -137,7 +143,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -149,12 +155,12 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable + fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix - std::vector axes_; //!< Which Euler angle axes we want to measure + std::vector axes_; //!< Which Euler angle axes we want to measure private: // Allow Boost Serialization access to private methods @@ -167,13 +173,13 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; - archive & axes_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; + archive& axes_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h index 5c1e81bed..5be4cba8e 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/absolute_pose_2d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/absolute_pose_2d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp index 0bc6347b1..00623124b 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -103,14 +102,12 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g. "{fuse_variables::Orientation2DStamped::Yaw}" */ AbsolutePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices = - {fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y}, // NOLINT - const std::vector & angular_indices = {fuse_variables::Orientation2DStamped::YAW}); // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + fuse_variables::Position2DStamped::Y }, // NOLINT + const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -123,7 +120,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * Order is (x, y, yaw). Note that the returned vector will be full sized (3x1) and in the stated * order. */ - const fuse_core::Vector3d & mean() const {return mean_;} + const fuse_core::Vector3d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -131,7 +131,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -147,7 +150,7 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -159,10 +162,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector3d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector3d mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -176,12 +179,12 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h index e5a3240ac..2f501a6f7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/absolute_pose_3d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/absolute_pose_3d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 51238c8b7..8fcc4925c 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -88,12 +87,9 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: x, y, z, qw, qx, qy, qz) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, qx, qy, qz) */ - AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance); + AbsolutePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector7d& mean, const fuse_core::Matrix6d& covariance); /** * @brief Destructor @@ -105,14 +101,20 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qw, qx, qy, qz) */ - const fuse_core::Vector7d & mean() const {return mean_;} + const fuse_core::Vector7d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z, qx, qy, qz) */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -129,7 +131,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -141,10 +143,10 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix private: @@ -158,12 +160,12 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp index 15f729ab7..f9c8ed372 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp @@ -65,7 +65,6 @@ #include #include - namespace fuse_constraints { @@ -103,14 +102,9 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] last_A Iterator pointing to one past the last A matrix * @param[in] b The b vector of the marginal cost (of the form A*(x - x_bar) + b) */ - template - MarginalConstraint( - const std::string & source, - VariableIterator first_variable, - VariableIterator last_variable, - MatrixIterator first_A, - MatrixIterator last_A, - const fuse_core::VectorXd & b); + template + MarginalConstraint(const std::string& source, VariableIterator first_variable, VariableIterator last_variable, + MatrixIterator first_A, MatrixIterator last_A, const fuse_core::VectorXd& b); /** * @brief Destructor @@ -120,23 +114,32 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the A matrices of the marginal constraint */ - const std::vector & A() const {return A_;} + const std::vector& A() const + { + return A_; + } /** * @brief Read-only access to the b vector of the marginal constraint */ - const fuse_core::VectorXd & b() const {return b_;} + const fuse_core::VectorXd& b() const + { + return b_; + } /** * @brief Read-only access to the variable linearization points, x_bar */ - const std::vector & x_bar() const {return x_bar_;} + const std::vector& x_bar() const + { + return x_bar_; + } #if !CERES_SUPPORTS_MANIFOLDS /** * @brief Read-only access to the variable local parameterizations */ - const std::vector & localParameterizations() const + const std::vector& localParameterizations() const { return local_parameterizations_; } @@ -144,7 +147,10 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the variable manifolds */ - const std::vector & manifolds() const {return manifolds_;} + const std::vector& manifolds() const + { + return manifolds_; + } #endif /** @@ -152,7 +158,7 @@ class MarginalConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -164,11 +170,11 @@ class MarginalConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: std::vector A_; //!< The A matrices of the marginal constraint - fuse_core::VectorXd b_; //!< The b vector of the marginal constraint + fuse_core::VectorXd b_; //!< The b vector of the marginal constraint #if !CERES_SUPPORTS_MANIFOLDS //!< The local parameterizations std::vector local_parameterizations_; @@ -187,8 +193,8 @@ class MarginalConstraint : public fuse_core::Constraint * @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 save(Archive & archive, const unsigned int /* version */) const + template + void save(Archive& archive, const unsigned int /* version */) const { archive << boost::serialization::base_object(*this); archive << A_; @@ -207,13 +213,14 @@ class MarginalConstraint : public fuse_core::Constraint * @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) + template + void load(Archive& archive, const unsigned int version) { archive >> boost::serialization::base_object(*this); archive >> A_; archive >> b_; - if (version == 0) { + 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 @@ -224,26 +231,25 @@ class MarginalConstraint : public fuse_core::Constraint #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));}); + 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. + } + 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."); + 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 @@ -259,7 +265,7 @@ namespace detail /** * @brief Return the UUID of the provided variable */ -inline const fuse_core::UUID getUuid(const fuse_core::Variable & variable) +inline const fuse_core::UUID getUuid(const fuse_core::Variable& variable) { return variable.uuid(); } @@ -267,7 +273,7 @@ inline const fuse_core::UUID getUuid(const fuse_core::Variable & variable) /** * @brief Return the current value of the provided variable */ -inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & variable) +inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& variable) { return Eigen::Map(variable.data(), variable.size()); } @@ -276,8 +282,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & var /** * @brief Return the local parameterization of the provided variable */ -inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization( - const fuse_core::Variable & variable) +inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(const fuse_core::Variable& variable) { return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization()); } @@ -285,7 +290,7 @@ inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization( /** * @brief Return the manifold of the provided variable */ -inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & variable) +inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& variable) { return fuse_core::Manifold::SharedPtr(variable.manifold()); } @@ -293,31 +298,27 @@ inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & va } // namespace detail -template -MarginalConstraint::MarginalConstraint( - const std::string & source, - VariableIterator first_variable, - VariableIterator last_variable, - MatrixIterator first_A, - MatrixIterator last_A, - const fuse_core::VectorXd & b) -: Constraint(source, - boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)), - A_(first_A, last_A), - b_(b), +template +MarginalConstraint::MarginalConstraint(const std::string& source, VariableIterator first_variable, + VariableIterator last_variable, MatrixIterator first_A, MatrixIterator last_A, + const fuse_core::VectorXd& b) + : Constraint(source, boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)) + , A_(first_A, last_A) + , b_(b) + , #if !CERES_SUPPORTS_MANIFOLDS - local_parameterizations_(boost::make_transform_iterator(first_variable, - &fuse_constraints::detail::getLocalParameterization), - boost::make_transform_iterator(last_variable, - &fuse_constraints::detail::getLocalParameterization)), + local_parameterizations_( + boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getLocalParameterization), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getLocalParameterization)) + , #else - manifolds_( - boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)), + manifolds_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)) + , #endif x_bar_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue)) + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue)) { assert(!A_.empty()); assert(A_.size() == x_bar_.size()); @@ -327,19 +328,11 @@ MarginalConstraint::MarginalConstraint( assert(A_.size() == manifolds_.size()); #endif assert(b_.rows() > 0); - assert( - std::all_of( - A_.begin(), A_.end(), [this](const auto & A) { - return A.rows() == this->b_.rows(); - })); // NOLINT - assert( - std::all_of( - boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)), - boost::make_zip_iterator(boost::make_tuple(A_.end(), last_variable)), - [](const boost::tuple & tuple) // NOLINT - { - return static_cast(tuple.get<0>().cols()) == tuple.get<1>().localSize(); - })); // NOLINT + assert(std::all_of(A_.begin(), A_.end(), [this](const auto& A) { return A.rows() == this->b_.rows(); })); // NOLINT + assert(std::all_of(boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)), + boost::make_zip_iterator(boost::make_tuple(A_.end(), last_variable)), + [](const boost::tuple& tuple) // NOLINT + { return static_cast(tuple.get<0>().cols()) == tuple.get<1>().localSize(); })); // NOLINT } } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp index 671ebf380..c8a734221 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp @@ -78,11 +78,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] local_parameterizations The local parameterization associated with the variable */ - MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & local_parameterizations); + MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& local_parameterizations); #else /** * @brief Construct a cost function instance @@ -92,11 +90,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] manifolds The manifold associated with the variable */ - MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & manifolds); + MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& manifolds); #endif /** @@ -108,21 +104,18 @@ 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 - const fuse_core::VectorXd & b_; //!< The b vector of the marginal cost + const std::vector& A_; //!< The A matrices of the marginal cost + const fuse_core::VectorXd& b_; //!< The b vector of the marginal cost #if !CERES_SUPPORTS_MANIFOLDS //!< Parameterizations - const std::vector & local_parameterizations_; + const std::vector& local_parameterizations_; #else - const std::vector & manifolds_; //!< Manifolds + const std::vector& manifolds_; //!< Manifolds #endif - const std::vector & x_bar_; //!< The linearization point of each variable + const std::vector& x_bar_; //!< The linearization point of each variable }; } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp index 1cc666243..6911376e6 100644 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp +++ b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp @@ -54,7 +54,6 @@ #include - namespace fuse_constraints { @@ -75,9 +74,8 @@ namespace fuse_constraints * least one marginalized variable * @return The mapping from variable UUID to the computed elimination order */ -UuidOrdering computeEliminationOrder( - const std::vector & marginalized_variables, - const fuse_core::Graph & graph); +UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, + const fuse_core::Graph& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -99,10 +97,9 @@ UuidOrdering computeEliminationOrder( * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph); +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -126,11 +123,10 @@ fuse_core::Transaction marginalizeVariables( * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph, - const fuse_constraints::UuidOrdering & elimination_order); +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph, + const fuse_constraints::UuidOrdering& elimination_order); namespace detail { @@ -160,10 +156,8 @@ struct LinearTerm * @return A LinearTerm consisting of Jacobian blocks associated with each involved variable in * elimination order */ -LinearTerm linearize( - const fuse_core::Constraint & constraint, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order); +LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, + const UuidOrdering& elimination_order); /** * @brief Marginalize out the lowest-ordered variable from the provided set of linear terms @@ -175,7 +169,7 @@ LinearTerm linearize( * index * @return A LinearTerm object containing the information on the remaining variables */ -LinearTerm marginalizeNext(const std::vector & linear_terms); +LinearTerm marginalizeNext(const std::vector& linear_terms); /** * @brief Convert the provided linear term into a MarginalConstraint @@ -187,11 +181,9 @@ LinearTerm marginalizeNext(const std::vector & linear_terms); * @param[in] elimination_order The mapping from variable UUID to LinearTerm variable index * @return An equivalent MarginalConstraint object */ -MarginalConstraint::SharedPtr createMarginalConstraint( - const std::string & source, - const LinearTerm & linear_term, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order); +MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, + const fuse_core::Graph& graph, + const UuidOrdering& elimination_order); } // namespace detail } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/normal_delta.hpp b/fuse_constraints/include/fuse_constraints/normal_delta.hpp index c9aad461d..8cf4ebd85 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ class NormalDelta : public ceres::CostFunction * these are the same type of variable. At a minimum, they must have the same * dimensions and the per-element subtraction operator must be valid. */ - NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorXd & b); + NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalDelta : public ceres::CostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h index 1a54e6c9a..b443ece34 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_delta_orientation_2d.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_2d.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp index b8e9e0505..5864f1962 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp @@ -36,7 +36,6 @@ #include - namespace fuse_constraints { @@ -80,10 +79,7 @@ class NormalDeltaOrientation2D : public ceres::SizedCostFunction<1, 1, 1> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: double A_; //!< The residual weighting matrix, most likely the square root information matrix diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h index 5e812868b..ae0a55424 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index 988c623bb..dcdebdba9 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_constraints { @@ -80,37 +79,21 @@ class NormalDeltaOrientation3DCostFunctor * order (x, y, z) * @param[in] b The measured change between the two orientation variables */ - NormalDeltaOrientation3DCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b) - : A_(A), - b_(b) + NormalDeltaOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation1, const T * const orientation2, T * residuals) const + template + bool operator()(const T* const orientation1, const T* const orientation2, T* residuals) const { using fuse_variables::Orientation3DStamped; - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; + T orientation1_inverse[4] = { orientation1[0], -orientation1[1], -orientation1[2], -orientation1[3] }; - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; + T observation_inverse[4] = { T(b_(0)), T(-b_(1)), T(-b_(2)), T(-b_(3)) }; T difference[4]; ceres::QuaternionProduct(orientation1_inverse, orientation2, difference); diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp index c69acb90a..5f41b980a 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -83,16 +82,13 @@ class NormalDeltaPose2D : public ceres::SizedCostFunction diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp index 5badc5c7b..ddbf7ddf6 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -85,18 +84,14 @@ class NormalDeltaPose2DCostFunctor * order (x, y, yaw) * @param[in] b The exposed pose difference in order (x, y, yaw) */ - NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const position2, + const T* const orientation2, T* residual) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root @@ -104,30 +99,22 @@ class NormalDeltaPose2DCostFunctor fuse_core::Vector3d b_; //!< The measured difference between variable x0 and variable x1 }; -NormalDeltaPose2DCostFunctor::NormalDeltaPose2DCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalDeltaPose2DCostFunctor::NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) + : A_(A), b_(b) { } -template -bool NormalDeltaPose2DCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose2DCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, T* residual) const { Eigen::Map> position1_vector(position1); Eigen::Map> position2_vector(position2); Eigen::Matrix full_residuals_vector; full_residuals_vector.template head<2>() = - fuse_core::rotationMatrix2D(orientation1[0]).transpose() * - (position2_vector - position1_vector) - - b_.head<2>().template cast(); + fuse_core::rotationMatrix2D(orientation1[0]).transpose() * (position2_vector - position1_vector) - + b_.head<2>().template cast(); full_residuals_vector(2) = fuse_core::wrapAngle2D(orientation2[0] - orientation1[0] - T(b_(2))); // Scale the residuals by the square root information matrix to account for diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h index 3d9c34dc7..cad6467d2 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_delta_pose_3d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_delta_pose_3d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index ace413e51..3a939ea64 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -78,18 +77,14 @@ class NormalDeltaPose3DCostFunctor * order (dx, dy, dz, dqx, dqy, dqz) * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) */ - NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const position2, + const T* const orientation2, T* residual) const; private: fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root @@ -99,43 +94,21 @@ class NormalDeltaPose3DCostFunctor NormalDeltaOrientation3DCostFunctor orientation_functor_; }; -NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor( - const fuse_core::Matrix6d & A, - const fuse_core::Vector7d & b) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will - // not be scaled +NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) + : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will + // not be scaled { } -template -bool NormalDeltaPose3DCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose3DCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, T* residual) const { // Compute the position delta between pose1 and pose2 - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - T position_delta[3] = - { - position2[0] - position1[0], - position2[1] - position1[1], - position2[2] - position1[2] - }; + T orientation1_inverse[4] = { orientation1[0], -orientation1[1], -orientation1[2], -orientation1[3] }; + T position_delta[3] = { position2[0] - position1[0], position2[1] - position1[1], position2[2] - position1[2] }; T position_delta_rotated[3]; - ceres::QuaternionRotatePoint( - orientation1_inverse, - position_delta, - position_delta_rotated); + ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); // Compute the first three residual terms as (position_delta - b) residual[0] = position_delta_rotated[0] - T(b_[0]); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h index 3feea4b61..d06011234 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_orientation_2d.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_2d.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp index c20723420..a870c59d9 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp @@ -36,7 +36,6 @@ #include - namespace fuse_constraints { @@ -82,10 +81,7 @@ class NormalPriorOrientation2D : public ceres::SizedCostFunction<1, 1> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: double A_; //!< The residual weighting matrix, most likely the square root information matrix diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h index 6e7add49e..6ac0a9810 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp index b2702ee2d..1ae1ad7c6 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_constraints { @@ -79,38 +78,22 @@ class NormalPriorOrientation3DCostFunctor * order (quaternion_x, quaternion_y, quaternion_z) * @param[in] b The orientation measurement or prior in order (w, x, y, z) */ - NormalPriorOrientation3DCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b) - : A_(A), - b_(b) + NormalPriorOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation, T * residuals) const + template + bool operator()(const T* const orientation, T* residuals) const { using fuse_variables::Orientation3DStamped; // Compute the delta quaternion - T variable[4] = - { - orientation[0], - orientation[1], - orientation[2], - orientation[3] - }; + T variable[4] = { orientation[0], orientation[1], orientation[2], orientation[3] }; - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; + T observation_inverse[4] = { T(b_(0)), T(-b_(1)), T(-b_(2)), T(-b_(3)) }; T difference[4]; ceres::QuaternionProduct(observation_inverse, variable, difference); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h index 33d3ab943..5abe413fe 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index fd49f06c5..851d65a58 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_constraints { @@ -86,52 +85,48 @@ class NormalPriorOrientation3DEulerCostFunctor * @param[in] b The orientation measurement or prior. Its order must match the values in \p axes. * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. */ - NormalPriorOrientation3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::VectorXd & b, - const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}) : //NOLINT - A_(A), - b_(b), - axes_(axes) + NormalPriorOrientation3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b, + const std::vector& axes = { Euler::ROLL, Euler::PITCH, Euler::YAW }) + : // NOLINT + A_(A) + , b_(b) + , axes_(axes) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation, T * residuals) const + template + bool operator()(const T* const orientation, T* residuals) const { using fuse_variables::Orientation3DStamped; - for (size_t i = 0; i < axes_.size(); ++i) { + for (size_t i = 0; i < axes_.size(); ++i) + { T angle; - switch (axes_[i]) { + switch (axes_[i]) + { case Euler::ROLL: - { - angle = fuse_core::getRoll( - orientation[0], orientation[1], orientation[2], - orientation[3]); - break; - } + { + angle = fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } case Euler::PITCH: - { - angle = - fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); - break; - } + { + angle = fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } case Euler::YAW: - { - angle = - fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); - break; - } + { + angle = fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } default: - { - throw std::runtime_error( - "The provided axis specified is unknown. " - "I should probably be more informative here"); - } + { + throw std::runtime_error("The provided axis specified is unknown. " + "I should probably be more informative here"); + } } residuals[i] = angle - T(b_[i]); } @@ -143,9 +138,9 @@ class NormalPriorOrientation3DEulerCostFunctor } private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix - fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value std::vector axes_; //!< The Euler angle axes that we're measuring }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp index bb25137f2..c8f262460 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -78,7 +77,7 @@ class NormalPriorPose2D : public ceres::SizedCostFunction * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Destructor @@ -89,10 +88,7 @@ class NormalPriorPose2D : public ceres::SizedCostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h index d2760c63c..83fd97112 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_pose_2d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_2d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp index 586a8146e..0d24bdb8f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -80,13 +79,13 @@ class NormalPriorPose2DCostFunctor * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root @@ -94,18 +93,13 @@ class NormalPriorPose2DCostFunctor fuse_core::Vector3d b_; //!< The measured 2D pose value }; -NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) + : A_(A), b_(b) { } -template -bool NormalPriorPose2DCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose2DCostFunctor::operator()(const T* const position, const T* const orientation, T* residual) const { Eigen::Matrix full_residuals_vector; full_residuals_vector(0) = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h index 1645808cf..a302c592b 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_pose_3d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_3d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index f8d20f96f..75bcfce05 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -79,13 +78,13 @@ class NormalPriorPose3DCostFunctor * order (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::Matrix6d A_; @@ -94,19 +93,13 @@ class NormalPriorPose3DCostFunctor NormalPriorOrientation3DCostFunctor orientation_functor_; }; -NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor( - const fuse_core::Matrix6d & A, - const fuse_core::Vector7d & b) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled +NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) + : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { } -template -bool NormalPriorPose3DCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose3DCostFunctor::operator()(const T* const position, const T* const orientation, T* residual) const { // Compute the position error residual[0] = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_constraint.hpp index 019a94e42..dda5d5a95 100644 --- a/fuse_constraints/include/fuse_constraints/relative_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_constraint.hpp @@ -57,7 +57,6 @@ #include #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ namespace fuse_constraints * is not the correct operation for a specific variable type (e.g. 3D rotations), a custom * constraint or template specialization will be needed. */ -template +template class RelativeConstraint : public fuse_core::Constraint { public: @@ -93,12 +92,8 @@ class RelativeConstraint : public fuse_core::Constraint * @param[in] delta The measured change between variable1 and variable2 * @param[in] covariance The measurement uncertainty */ - RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance); + RelativeConstraint(const std::string& source, const Variable& variable1, const Variable& variable2, + const fuse_core::VectorXd& delta, const fuse_core::MatrixXd& covariance); /** * @brief Constructor @@ -116,13 +111,9 @@ class RelativeConstraint : public fuse_core::Constraint * order defined by \p indices. * @param[in] indices The set of indices corresponding to the measured dimensions */ - RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance, - const std::vector & indices); + RelativeConstraint(const std::string& source, const Variable& variable1, const Variable& variable2, + const fuse_core::VectorXd& delta, const fuse_core::MatrixXd& covariance, + const std::vector& indices); /** * @brief Destructor @@ -136,7 +127,10 @@ class RelativeConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd & delta() const {return delta_;} + const fuse_core::VectorXd& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. @@ -146,7 +140,10 @@ class RelativeConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -163,7 +160,7 @@ class RelativeConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -175,10 +172,10 @@ class RelativeConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd delta_; //!< The measured change between the two variables + fuse_core::VectorXd delta_; //!< The measured change between the two variables fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -192,28 +189,23 @@ class RelativeConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; // Define unique names for the different variations of the absolute constraint -using RelativeAccelerationAngular2DStampedConstraint = - RelativeConstraint; -using RelativeAccelerationLinear2DStampedConstraint = - RelativeConstraint; -using RelativeOrientation2DStampedConstraint = - RelativeConstraint; +using RelativeAccelerationAngular2DStampedConstraint = RelativeConstraint; +using RelativeAccelerationLinear2DStampedConstraint = RelativeConstraint; +using RelativeOrientation2DStampedConstraint = RelativeConstraint; using RelativePosition2DStampedConstraint = RelativeConstraint; using RelativePosition3DStampedConstraint = RelativeConstraint; -using RelativeVelocityAngular2DStampedConstraint = - RelativeConstraint; -using RelativeVelocityLinear2DStampedConstraint = - RelativeConstraint; +using RelativeVelocityAngular2DStampedConstraint = RelativeConstraint; +using RelativeVelocityLinear2DStampedConstraint = RelativeConstraint; } // namespace fuse_constraints // Include the template implementation diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp index 46f8596eb..b39b83535 100644 --- a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp @@ -42,20 +42,17 @@ #include #include - namespace fuse_constraints { -template -RelativeConstraint::RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance) -: fuse_core::Constraint(source, {variable1.uuid(), variable2.uuid()}), // NOLINT(whitespace/braces) - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) +template +RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, + const Variable& variable2, const fuse_core::VectorXd& delta, + const fuse_core::MatrixXd& covariance) + : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) + , // NOLINT(whitespace/braces) + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { assert(variable1.size() == variable2.size()); assert(delta.rows() == static_cast(variable1.size())); @@ -63,15 +60,12 @@ RelativeConstraint::RelativeConstraint( assert(covariance.cols() == static_cast(variable1.size())); } -template -RelativeConstraint::RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices) -: fuse_core::Constraint(source, {variable1.uuid(), variable2.uuid()}) // NOLINT(whitespace/braces) +template +RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, + const Variable& variable2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& indices) + : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) // NOLINT(whitespace/braces) { assert(variable1.size() == variable2.size()); assert(partial_delta.rows() == static_cast(indices.size())); @@ -90,13 +84,14 @@ RelativeConstraint::RelativeConstraint( // dimensions, and the columns are in the order defined by the variable. delta_ = fuse_core::VectorXd::Zero(variable1.size()); sqrt_information_ = fuse_core::MatrixXd::Zero(indices.size(), variable1.size()); - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { delta_(indices[i]) = partial_delta(i); sqrt_information_.col(indices[i]) = partial_sqrt_information.col(i); } } -template +template fuse_core::MatrixXd RelativeConstraint::covariance() const { // We want to compute: @@ -111,8 +106,8 @@ fuse_core::MatrixXd RelativeConstraint::covariance() const return pinv * pinv.transpose(); } -template -void RelativeConstraint::print(std::ostream & stream) const +template +void RelativeConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -122,68 +117,66 @@ void RelativeConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -template -ceres::CostFunction * RelativeConstraint::costFunction() const +template +ceres::CostFunction* RelativeConstraint::costFunction() const { // Create a Gaussian/Normal Delta constraint return new fuse_constraints::NormalDelta(sqrt_information_, delta_); } // Specialization for Orientation2D -template<> -inline ceres::CostFunction * RelativeConstraint< - fuse_variables::Orientation2DStamped ->::costFunction() -const +template <> +inline ceres::CostFunction* RelativeConstraint::costFunction() const { // Create a Gaussian/Normal Delta constraint return new NormalDeltaOrientation2D(sqrt_information_(0, 0), delta_(0)); } // Specialize the type() method to return the name that is registered with the plugins -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeAccelerationAngular2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeAccelerationLinear2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeOrientation2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativePosition2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativePosition3DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeVelocityAngular2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeVelocityLinear2DStampedConstraint"; diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h index 361b147f1..58e3d3e9b 100644 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/relative_orientation_3d_stamped_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/relative_orientation_3d_stamped_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp index 6d68d424c..ba5c32f8f 100644 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_constraints { @@ -83,12 +82,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * (4x1 vector: w, x, y, z) * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector4d & delta, - const fuse_core::Matrix3d & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector4d& delta, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -99,12 +96,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as an Eigen quaternion * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const Eigen::Quaterniond & delta, - const fuse_core::Matrix3d & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const Eigen::Quaterniond& delta, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -115,12 +110,11 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as a ROS quaternion message * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const geometry_msgs::msg::Quaternion & delta, - const std::array & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const geometry_msgs::msg::Quaternion& delta, + const std::array& covariance); /** * @brief Destructor @@ -132,14 +126,20 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d & delta() const {return delta_;} + const fuse_core::Vector4d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z) */ - const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix3d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -153,7 +153,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -165,7 +165,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: /** @@ -174,7 +174,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond & quaternion); + static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d @@ -182,7 +182,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion & quaternion); + static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix @@ -190,9 +190,9 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] covariance The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array & covariance); + static fuse_core::Matrix3d toEigen(const std::array& covariance); - fuse_core::Vector4d delta_; //!< The measured/prior mean vector for this variable + fuse_core::Vector4d delta_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix private: @@ -206,12 +206,12 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h index ddb8d837d..267ed2a15 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/relative_pose_2d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/relative_pose_2d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp index a26368214..54113de52 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -106,16 +105,13 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g., "{fuse_variables::Orientation2DStamped::Yaw}" */ RelativePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & orientation1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & orientation2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices = - {fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y}, // NOLINT - const std::vector & angular_indices = {fuse_variables::Orientation2DStamped::YAW}); // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + fuse_variables::Position2DStamped::Y }, // NOLINT + const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -128,7 +124,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * Order is (dx, dy, dyaw). Note that the returned vector will be full sized (3x1) and in the * stated order. */ - const fuse_core::Vector3d & delta() const {return delta_;} + const fuse_core::Vector3d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. @@ -136,7 +135,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -152,7 +154,7 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -164,10 +166,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector3d delta_; //!< The measured pose change (dx, dy, dyaw) + fuse_core::Vector3d delta_; //!< The measured pose change (dx, dy, dyaw) fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -182,12 +184,12 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h index 96167dab2..a2dee13d6 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/relative_pose_3d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/relative_pose_3d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index 21155fc4c..c0a5ff72e 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -85,14 +84,11 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) */ - RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::Matrix6d & covariance); + RelativePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector7d& delta, const fuse_core::Matrix6d& covariance); /** * @brief Destructor @@ -102,12 +98,18 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector7d & delta() const {return delta_;} + const fuse_core::Vector7d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -122,7 +124,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -134,10 +136,10 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -152,12 +154,12 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp old mode 100755 new mode 100644 index 401d8db06..1140c3884 --- a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp +++ b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -86,7 +85,7 @@ class UuidOrdering * @param[in] first Iterator pointing to the first UUID to add to the ordering * @param[in] last Iterator pointing to one past the last UUID to add to the ordering */ - template + template UuidOrdering(UuidConstIterator first, UuidConstIterator last); /** @@ -109,7 +108,7 @@ class UuidOrdering /** * @brief Return true if the UUID exists in the ordering */ - bool exists(const fuse_core::UUID & uuid) const; + bool exists(const fuse_core::UUID& uuid) const; /** * @brief Add a new UUID to the back of the ordering @@ -119,46 +118,47 @@ class UuidOrdering * @param[in] uuid The UUID to insert * @return True if the UUID was inserted, false if the UUID already existed */ - bool push_back(const fuse_core::UUID & uuid); + bool push_back(const fuse_core::UUID& uuid); /** * @brief Access the UUID stored at the provided index * * Accessing an index that does not exist results in undefined behavior */ - const fuse_core::UUID & operator[](const unsigned int index) const; + const fuse_core::UUID& operator[](const unsigned int index) const; /** * @brief Access the index associated with the provided UUID * * Accessing a UUID that does not exist results in the provided UUID being added to the ordering */ - unsigned int operator[](const fuse_core::UUID & uuid); + unsigned int operator[](const fuse_core::UUID& uuid); /** * @brief Access the UUID stored at the provided index * * If the requested index does not exist, an out_of_range exception will be thrown. */ - const fuse_core::UUID & at(const unsigned int index) const; + const fuse_core::UUID& at(const unsigned int index) const; /** * @brief Access the index associated with the provided UUID * * If the requested UUID does not exist, an out_of_range exception will be thrown. */ - unsigned int at(const fuse_core::UUID & uuid) const; + unsigned int at(const fuse_core::UUID& uuid) const; private: - using UuidOrderMapping = boost::bimaps::bimap, - boost::bimaps::unordered_set_of>; + using UuidOrderMapping = + boost::bimaps::bimap, boost::bimaps::unordered_set_of>; UuidOrderMapping order_; //!< Collection that contains the Index<-->UUID mapping }; -template +template UuidOrdering::UuidOrdering(UuidConstIterator first, UuidConstIterator last) { - for (; first != last; ++first) { + for (; first != last; ++first) + { order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), *first)); } } diff --git a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp old mode 100755 new mode 100644 index cd29f6c6c..c83d66708 --- a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp +++ b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -90,10 +89,8 @@ class VariableConstraints /** * @brief Add this constraint to all variables in the provided range */ - template - void insert( - const unsigned int constraint, VariableIndexIterator first, - VariableIndexIterator last); + template + void insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last); /** * @brief Add a single orphan variable, i.e. a variable without constraints @@ -106,7 +103,7 @@ class VariableConstraints * * Accessing a variable id that is not part of this container results in undefined behavior */ - template + template OutputIterator getConstraints(const unsigned int variable_id, OutputIterator result) const; /** @@ -114,7 +111,7 @@ class VariableConstraints * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; private: using ConstraintCollection = std::unordered_set; @@ -123,29 +120,26 @@ class VariableConstraints ConstraintsByVariable variable_constraints_; //!< The collection of constraints for each variable }; -template -void VariableConstraints::insert( - const unsigned int constraint, VariableIndexIterator first, - VariableIndexIterator last) +template +void VariableConstraints::insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last) { - for (; first != last; ++first) { + for (; first != last; ++first) + { insert(constraint, *first); } } -template -OutputIterator VariableConstraints::getConstraints( - const unsigned int variable_id, - OutputIterator result) const +template +OutputIterator VariableConstraints::getConstraints(const unsigned int variable_id, OutputIterator result) const { - const auto & constraints = variable_constraints_[variable_id]; + const auto& constraints = variable_constraints_[variable_id]; return std::copy(std::begin(constraints), std::end(constraints), result); } /** * Stream operator for printing VariableConstraints objects. */ -std::ostream & operator<<(std::ostream & stream, const VariableConstraints & variable_constraints); +std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints); } // namespace fuse_constraints diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..b30a4b77b 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -43,24 +43,10 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstrai BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 0131bfd20..2091e8527 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -45,31 +45,26 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::Matrix3d & covariance) -: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector4d& mean, + const fuse_core::Matrix3d& covariance) + : fuse_core::Constraint(source, { orientation.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const Eigen::Quaterniond & mean, - const fuse_core::Matrix3d & covariance) -: AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), covariance) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const Eigen::Quaterniond& mean, + const fuse_core::Matrix3d& covariance) + : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), covariance) { } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const geometry_msgs::msg::Quaternion & mean, - const std::array & covariance) -: AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), toEigen(covariance)) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, + const geometry_msgs::msg::Quaternion& mean, const std::array& covariance) + : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), toEigen(covariance)) { } @@ -78,7 +73,7 @@ fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::covariance() const return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const +void AbsoluteOrientation3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -87,37 +82,34 @@ void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsoluteOrientation3DStampedConstraint::costFunction() const +ceres::CostFunction* AbsoluteOrientation3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); + new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( - const Eigen::Quaterniond & quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( - const geometry_msgs::msg::Quaternion & quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen( - const std::array & covariance) +fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen(const std::array& covariance) { return fuse_core::Matrix3d(covariance.data()); } @@ -125,6 +117,4 @@ fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen( } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation3DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp index d9fb6dae5..29ba0d652 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -46,15 +46,13 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedEulerConstraint::AbsoluteOrientation3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & axes) -: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()), - axes_(axes) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::VectorXd& mean, + const fuse_core::MatrixXd& covariance, const std::vector& axes) + : fuse_core::Constraint(source, { orientation.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) + , axes_(axes) { assert(covariance.rows() == static_cast(axes.size())); assert(covariance.cols() == static_cast(axes.size())); @@ -66,7 +64,7 @@ fuse_core::MatrixXd AbsoluteOrientation3DStampedEulerConstraint::covariance() co return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream & stream) const +void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -75,22 +73,20 @@ void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream & stream) c << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsoluteOrientation3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* AbsoluteOrientation3DStampedEulerConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DEulerCostFunctor(sqrt_information_, mean_, axes_), axes_.size()); + return new ceres::AutoDiffCostFunction( + new NormalPriorOrientation3DEulerCostFunctor(sqrt_information_, mean_, axes_), axes_.size()); } } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 1b84d5f1b..23bd3c566 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -46,14 +46,11 @@ namespace fuse_constraints { AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT(whitespace/braces) + const std::string& source, const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, + const std::vector& angular_indices) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) // NOLINT(whitespace/braces) { size_t total_variable_size = position.size() + orientation.size(); size_t total_indices = linear_indices.size() + angular_indices.size(); @@ -75,12 +72,14 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( // and the columns are in the order defined by the variable. mean_ = fuse_core::VectorXd::Zero(total_variable_size); sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); - for (size_t i = 0; i < linear_indices.size(); ++i) { + for (size_t i = 0; i < linear_indices.size(); ++i) + { mean_(linear_indices[i]) = partial_mean(i); sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); } - for (size_t i = linear_indices.size(); i < total_indices; ++i) { + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { size_t final_index = position.size() + angular_indices[i - linear_indices.size()]; mean_(final_index) = partial_mean(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); @@ -101,7 +100,7 @@ fuse_core::Matrix3d AbsolutePose2DStampedConstraint::covariance() const return pinv * pinv.transpose(); } -void AbsolutePose2DStampedConstraint::print(std::ostream & stream) const +void AbsolutePose2DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -111,13 +110,14 @@ void AbsolutePose2DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose2DStampedConstraint::costFunction() const +ceres::CostFunction* AbsolutePose2DStampedConstraint::costFunction() const { return new NormalPriorPose2D(sqrt_information_, mean_); } diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index cc64e8d75..41d91c68f 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -44,19 +44,19 @@ namespace fuse_constraints { -AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) +AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint(const std::string& source, + const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const +void AbsolutePose3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -66,16 +66,17 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const +ceres::CostFunction* AbsolutePose3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); + new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index 0ee0e937e..a04225933 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -45,29 +45,34 @@ namespace fuse_constraints { -void MarginalConstraint::print(std::ostream & stream) const +void MarginalConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" << " uuid: " << uuid() << "\n" << " variable:\n"; - for (const auto & variable : variables()) { + for (const auto& variable : variables()) + { stream << " - " << variable << "\n"; } 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"; + 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 << " b:\n" << b().format(indent) << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * MarginalConstraint::costFunction() const +ceres::CostFunction* MarginalConstraint::costFunction() const { #if !CERES_SUPPORTS_MANIFOLDS return new MarginalCostFunction(A_, b_, x_bar_, local_parameterizations_); diff --git a/fuse_constraints/src/marginal_cost_function.cpp b/fuse_constraints/src/marginal_cost_function.cpp index d148a5043..34274cca3 100644 --- a/fuse_constraints/src/marginal_cost_function.cpp +++ b/fuse_constraints/src/marginal_cost_function.cpp @@ -46,57 +46,53 @@ namespace fuse_constraints { #if !CERES_SUPPORTS_MANIFOLDS MarginalCostFunction::MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & local_parameterizations) -: A_(A), - b_(b), - local_parameterizations_(local_parameterizations), - x_bar_(x_bar) + const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& local_parameterizations) + : A_(A), b_(b), local_parameterizations_(local_parameterizations), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto & x_bar : x_bar_) { + for (const auto& x_bar : x_bar_) + { mutable_parameter_block_sizes()->push_back(x_bar.size()); } } #else -MarginalCostFunction::MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & manifolds) -: A_(A), - b_(b), - manifolds_(manifolds), - x_bar_(x_bar) +MarginalCostFunction::MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& manifolds) + : A_(A), b_(b), manifolds_(manifolds), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto & x_bar : x_bar_) { + for (const auto& x_bar : x_bar_) + { mutable_parameter_block_sizes()->push_back(x_bar.size()); } } #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()); residuals_map = b_; - for (size_t i = 0; i < A_.size(); ++i) { + for (size_t i = 0; i < A_.size(); ++i) + { fuse_core::VectorXd delta(A_[i].cols()); #if !CERES_SUPPORTS_MANIFOLDS - if (local_parameterizations_[i]) { + if (local_parameterizations_[i]) + { local_parameterizations_[i]->Minus(x_bar_[i].data(), parameters[i], delta.data()); #else - if (manifolds_[i]) { + if (manifolds_[i]) + { manifolds_[i]->Minus(parameters[i], x_bar_[i].data(), delta.data()); #endif - } else { - for (int j = 0; j < x_bar_[i].rows(); ++j) { + } + else + { + for (int j = 0; j < x_bar_[i].rows(); ++j) + { delta[j] = parameters[i][j] - x_bar_[i][j]; } } @@ -104,29 +100,30 @@ bool MarginalCostFunction::Evaluate( } // Compute requested Jacobians - if (jacobians) { - for (size_t i = 0; i < A_.size(); ++i) { - if (jacobians[i]) { + if (jacobians) + { + for (size_t i = 0; i < A_.size(); ++i) + { + if (jacobians[i]) + { #if !CERES_SUPPORTS_MANIFOLDS - if (local_parameterizations_[i]) { - const auto & local_parameterization = local_parameterizations_[i]; - fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), - local_parameterization->GlobalSize()); + if (local_parameterizations_[i]) + { + const auto& local_parameterization = local_parameterizations_[i]; + fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), local_parameterization->GlobalSize()); local_parameterization->ComputeMinusJacobian(parameters[i], J_local.data()); #else - if (manifolds_[i]) { - const auto & manifold = manifolds_[i]; - fuse_core::MatrixXd J_local(manifold->TangentSize(), - manifold->AmbientSize()); + if (manifolds_[i]) + { + const auto& manifold = manifolds_[i]; + fuse_core::MatrixXd J_local(manifold->TangentSize(), manifold->AmbientSize()); manifold->MinusJacobian(parameters[i], J_local.data()); #endif - Eigen::Map( - jacobians[i], num_residuals(), - parameter_block_sizes()[i]) = A_[i] * J_local; - } else { - Eigen::Map( - jacobians[i], num_residuals(), - parameter_block_sizes()[i]) = A_[i]; + Eigen::Map(jacobians[i], num_residuals(), parameter_block_sizes()[i]) = A_[i] * J_local; + } + else + { + Eigen::Map(jacobians[i], num_residuals(), parameter_block_sizes()[i]) = A_[i]; } } } diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 1614cfa5d..f3fa47a32 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -45,7 +45,7 @@ // The ROS 2 ament linter incorrectly recognizes Eigen includes as C instead of C++ #include #include -#include // NOLINT[build/include_order] +#include // NOLINT[build/include_order] #include // NOLINT[build/include_order] #include @@ -59,9 +59,8 @@ namespace fuse_constraints { -UuidOrdering computeEliminationOrder( - const std::vector & marginalized_variables, - const fuse_core::Graph & graph) +UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, + const fuse_core::Graph& graph) { // COLAMD wants a somewhat weird structure // Variables are numbered sequentially in some arbitrary order. We call this the "variable index" @@ -83,20 +82,26 @@ UuidOrdering computeEliminationOrder( auto variable_order = UuidOrdering(); auto constraint_order = UuidOrdering(); auto variable_constraints = VariableConstraints(); - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { // Get all connected constraints to this variable const auto constraints = graph.getConnectedConstraints(variable_uuid); // If the variable is orphan (it has no constraints), add it to the VariableConstraints object // without constraints New variable index is automatically generated - if (boost::empty(constraints)) { + if (boost::empty(constraints)) + { variable_constraints.insert(variable_order[variable_uuid]); - } else { + } + else + { // Add each constraint to the VariableConstraints object // New constraint and variable indices are automatically generated - for (const auto & constraint : constraints) { + for (const auto& constraint : constraints) + { unsigned int constraint_index = constraint_order[constraint.uuid()]; - for (const auto & constraint_variable_uuid : constraint.variables()) { + for (const auto& constraint_variable_uuid : constraint.variables()) + { variable_constraints.insert(constraint_index, variable_order[constraint_variable_uuid]); } } @@ -104,10 +109,8 @@ 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); @@ -116,7 +119,8 @@ UuidOrdering computeEliminationOrder( auto p_iter = p.begin(); *p_iter = 0; ++p_iter; - for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index) { + for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index) + { A_iter = variable_constraints.getConstraints(variable_index, A_iter); *p_iter = std::distance(A.begin(), A_iter); ++p_iter; @@ -125,7 +129,8 @@ UuidOrdering computeEliminationOrder( // Define the variable groups used by CCOLAMD. All of the marginalized variables should be group0, // all the rest should be group1. std::vector variable_groups(variable_order.size(), 1); // Default all variables to group1 - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { // Reassign the marginalized variables to group0 variable_groups[variable_order.at(variable_uuid)] = 0; } @@ -136,16 +141,10 @@ UuidOrdering computeEliminationOrder( int stats[CCOLAMD_STATS]; // Finally call CCOLAMD - auto success = ccolamd( - constraint_order.size(), - variable_order.size(), - recommended_size, - A.data(), - p.data(), - knobs, - stats, - variable_groups.data()); - if (!success) { + auto success = ccolamd(constraint_order.size(), variable_order.size(), recommended_size, A.data(), p.data(), knobs, + stats, variable_groups.data()); + if (!success) + { throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); } @@ -153,30 +152,26 @@ UuidOrdering computeEliminationOrder( // CCOLAMD returns the elimination order by updating the values stored in p with the variable // index Remember that p is larger than variable_order.size() auto elimination_order = UuidOrdering(); - for (size_t i = 0ul; i < variable_order.size(); ++i) { + for (size_t i = 0ul; i < variable_order.size(); ++i) + { elimination_order.push_back(variable_order[p[i]]); } return elimination_order; } -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph) +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph) { - return marginalizeVariables( - source, - marginalized_variables, - graph, - computeEliminationOrder(marginalized_variables, graph)); + return marginalizeVariables(source, marginalized_variables, graph, + computeEliminationOrder(marginalized_variables, graph)); } -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph, - const fuse_constraints::UuidOrdering & elimination_order) +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph, + const fuse_constraints::UuidOrdering& elimination_order) { // TODO(swilliams) The method used to marginalize variables assumes that all variables are fully // constrained. However, with the introduction of "variables held constant", it is @@ -185,20 +180,17 @@ fuse_core::Transaction marginalizeVariables( // linearization and solve steps. A similar approach should be implemented here, // but that will require a major refactor. - assert( - std::all_of( - marginalized_variables.begin(), - marginalized_variables.end(), - [&elimination_order, &marginalized_variables](const fuse_core::UUID & variable_uuid) - { - return elimination_order.exists(variable_uuid) && - elimination_order.at(variable_uuid) < marginalized_variables.size(); - })); // NOLINT + assert(std::all_of(marginalized_variables.begin(), marginalized_variables.end(), + [&elimination_order, &marginalized_variables](const fuse_core::UUID& variable_uuid) { + return elimination_order.exists(variable_uuid) && + elimination_order.at(variable_uuid) < marginalized_variables.size(); + })); // NOLINT fuse_core::Transaction transaction; // Mark all of the marginalized variables for removal - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { transaction.removeVariable(variable_uuid); } @@ -208,13 +200,17 @@ fuse_core::Transaction marginalizeVariables( // Linearize all involved constraints, and store them with the variable where they will be used auto used_constraints = std::unordered_set(); std::vector> linear_terms(variable_order.size()); - for (size_t i = 0ul; i < marginalized_variables.size(); ++i) { + for (size_t i = 0ul; i < marginalized_variables.size(); ++i) + { const auto constraints = graph.getConnectedConstraints(variable_order[i]); - for (const auto & constraint : constraints) { - if (used_constraints.find(constraint.uuid()) == used_constraints.end()) { + for (const auto& constraint : constraints) + { + if (used_constraints.find(constraint.uuid()) == used_constraints.end()) + { used_constraints.insert(constraint.uuid()); // Ensure all connected variables are added to the ordering - for (const auto & variable_uuid : constraint.variables()) { + for (const auto& variable_uuid : constraint.variables()) + { variable_order.push_back(variable_uuid); } // Add the linearized constraint to the lowest-ordered connected variable @@ -233,20 +229,22 @@ fuse_core::Transaction marginalizeVariables( // Use the linearized constraints to marginalize each variable in order // Place the resulting marginal in the linear constraint bucket associated with the lowest-ordered // remaining variable - for (size_t i = 0ul; i < marginalized_variables.size(); ++i) { + for (size_t i = 0ul; i < marginalized_variables.size(); ++i) + { auto linear_marginal = detail::marginalizeNext(linear_terms[i]); - if (!linear_marginal.variables.empty()) { + if (!linear_marginal.variables.empty()) + { auto lowest_ordered_variable = linear_marginal.variables.front(); linear_terms[lowest_ordered_variable].push_back(std::move(linear_marginal)); } } // Convert all remaining linear marginals into marginal constraints - for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i) { - for (const auto & linear_term : linear_terms[i]) { - auto marginal_constraint = detail::createMarginalConstraint( - source, linear_term, graph, - variable_order); + for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i) + { + for (const auto& linear_term : linear_terms[i]) + { + auto marginal_constraint = detail::createMarginalConstraint(source, linear_term, graph, variable_order); transaction.addConstraint(std::move(marginal_constraint)); } } @@ -275,10 +273,8 @@ namespace detail * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/residual_block.cc * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc */ -LinearTerm linearize( - const fuse_core::Constraint & constraint, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order) +LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, + const UuidOrdering& elimination_order) { LinearTerm result; @@ -290,16 +286,17 @@ LinearTerm linearize( // * Generate a vector of variable value pointers. This is needed for the Ceres API. // * Allocate a matrix for each jacobian block. We will have Ceres populate the matrix. // * Generate a vector of jacobian pointers. This is needed for the Ceres API. - const auto & variable_uuids = constraint.variables(); + const auto& variable_uuids = constraint.variables(); const size_t variable_count = variable_uuids.size(); - std::vector variable_values; + std::vector variable_values; variable_values.reserve(variable_count); - std::vector jacobians; + std::vector jacobians; jacobians.reserve(variable_count); result.variables.reserve(variable_count); result.A.reserve(variable_count); - for (const auto & variable_uuid : variable_uuids) { - const auto & variable = graph.getVariable(variable_uuid); + for (const auto& variable_uuid : variable_uuids) + { + const auto& variable = graph.getVariable(variable_uuid); variable_values.push_back(variable.data()); result.variables.push_back(elimination_order.at(variable_uuid)); result.A.push_back(fuse_core::MatrixXd(row_count, variable.size())); @@ -311,15 +308,16 @@ LinearTerm linearize( bool success = cost_function->Evaluate(variable_values.data(), result.b.data(), jacobians.data()); delete cost_function; success = success && result.b.array().isFinite().all(); - for (const auto & A : result.A) { + for (const auto& A : result.A) + { success = success && A.array().isFinite().all(); } - if (!success) { - throw std::runtime_error( - "Error in evaluating the cost function. There are two possible reasons. " - "Either the CostFunction did not evaluate and fill all residual and jacobians " - "that were requested or there was a non-finite value (nan/infinite) generated " - "during the jacobian computation."); + if (!success) + { + throw std::runtime_error("Error in evaluating the cost function. There are two possible reasons. " + "Either the CostFunction did not evaluate and fill all residual and jacobians " + "that were requested or there was a non-finite value (nan/infinite) generated " + "during the jacobian computation."); } // Update the Jacobians with the local parameterizations. This potentially changes the size of the @@ -328,40 +326,50 @@ LinearTerm linearize( // local parameterization is applied. We also check for variables that have been marked as // constants. Since these variables cannot change value, their derivatives/Jacobians should be // zero. - for (size_t index = 0ul; index < variable_count; ++index) { - const auto & variable_uuid = variable_uuids[index]; - const auto & variable = graph.getVariable(variable_uuid); + for (size_t index = 0ul; index < variable_count; ++index) + { + const auto& variable_uuid = variable_uuids[index]; + const auto& variable = graph.getVariable(variable_uuid); #if !CERES_SUPPORTS_MANIFOLDS auto local_parameterization = variable.localParameterization(); - auto & jacobian = result.A[index]; - if (variable.holdConstant()) { - if (local_parameterization) { + auto& jacobian = result.A[index]; + if (variable.holdConstant()) + { + if (local_parameterization) + { jacobian.resize(Eigen::NoChange, local_parameterization->LocalSize()); } jacobian.setZero(); - } else if (local_parameterization) { - fuse_core::MatrixXd J(local_parameterization->GlobalSize(), - local_parameterization->LocalSize()); + } + else if (local_parameterization) + { + fuse_core::MatrixXd J(local_parameterization->GlobalSize(), local_parameterization->LocalSize()); local_parameterization->ComputeJacobian(variable_values[index], J.data()); jacobian *= J; } - if (local_parameterization) { + if (local_parameterization) + { delete local_parameterization; } #else auto manifold = variable.manifold(); - auto & jacobian = result.A[index]; - if (variable.holdConstant()) { - if (manifold) { + auto& jacobian = result.A[index]; + if (variable.holdConstant()) + { + if (manifold) + { jacobian.resize(Eigen::NoChange, manifold->TangentSize()); } jacobian.setZero(); - } else if (manifold) { + } + else if (manifold) + { fuse_core::MatrixXd J(manifold->AmbientSize(), manifold->TangentSize()); manifold->PlusJacobian(variable_values[index], J.data()); jacobian *= J; } - if (manifold) { + if (manifold) + { delete manifold; } #endif @@ -369,29 +377,35 @@ LinearTerm linearize( // Correct A and b for the effects of the loss function auto loss_function = constraint.lossFunction(); - if (loss_function) { + if (loss_function) + { double squared_norm = result.b.squaredNorm(); double rho[3]; loss_function->Evaluate(squared_norm, rho); - if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { + if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) + { delete loss_function; } double sqrt_rho1 = std::sqrt(rho[1]); double alpha = 0.0; - if ((squared_norm > 0.0) && (rho[2] > 0.0)) { + if ((squared_norm > 0.0) && (rho[2] > 0.0)) + { const double D = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; alpha = 1.0 - std::sqrt(D); } // Correct the Jacobians - for (auto & jacobian : result.A) { - if (alpha == 0.0) { + for (auto& jacobian : result.A) + { + if (alpha == 0.0) + { jacobian *= sqrt_rho1; - } else { + } + else + { // TODO(swilliams) This may be inefficient, at least according to notes in the Ceres // codebase. - jacobian = sqrt_rho1 * - (jacobian - (alpha / squared_norm) * result.b * (result.b.transpose() * jacobian)); + jacobian = sqrt_rho1 * (jacobian - (alpha / squared_norm) * result.b * (result.b.transpose() * jacobian)); } } @@ -402,9 +416,10 @@ LinearTerm linearize( return result; } -LinearTerm marginalizeNext(const std::vector & linear_terms) +LinearTerm marginalizeNext(const std::vector& linear_terms) { - if (linear_terms.empty()) { + if (linear_terms.empty()) + { return {}; } @@ -415,19 +430,17 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) // because the number of variables is assumed to be small. You need 1000s of variables before the // std::set outperforms the std::vector. auto dense_to_index = std::vector(); - for (const auto & linear_term : linear_terms) { - std::copy( - linear_term.variables.begin(), linear_term.variables.end(), - std::back_inserter(dense_to_index)); + for (const auto& linear_term : linear_terms) + { + std::copy(linear_term.variables.begin(), linear_term.variables.end(), std::back_inserter(dense_to_index)); } std::sort(dense_to_index.begin(), dense_to_index.end()); - dense_to_index.erase( - std::unique(dense_to_index.begin(), dense_to_index.end()), - dense_to_index.end()); + dense_to_index.erase(std::unique(dense_to_index.begin(), dense_to_index.end()), dense_to_index.end()); // Construct the inverse mapping auto index_to_dense = std::vector(dense_to_index.back() + 1, 0); - for (size_t dense = 0ul; dense < dense_to_index.size(); ++dense) { + for (size_t dense = 0ul; dense < dense_to_index.size(); ++dense) + { index_to_dense[dense_to_index[dense]] = dense; } @@ -435,14 +448,17 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto row_offsets = std::vector(); row_offsets.reserve(linear_terms.size() + 1ul); row_offsets.push_back(0u); - for (const auto & linear_term : linear_terms) { + for (const auto& linear_term : linear_terms) + { row_offsets.push_back(row_offsets.back() + linear_term.b.rows()); } // Compute the column offsets auto index_to_cols = std::vector(dense_to_index.back() + 1u, 0u); - for (const auto & linear_term : linear_terms) { - for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { + for (const auto& linear_term : linear_terms) + { + for (size_t i = 0ul; i < linear_term.variables.size(); ++i) + { auto index = linear_term.variables[i]; index_to_cols[index] = linear_term.A[i].cols(); } @@ -451,29 +467,34 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto column_offsets = std::vector(); column_offsets.reserve(dense_to_index.size() + 1ul); column_offsets.push_back(0u); - for (size_t dense = 0; dense < dense_to_index.size(); ++dense) { + for (size_t dense = 0; dense < dense_to_index.size(); ++dense) + { column_offsets.push_back(column_offsets.back() + index_to_cols[dense_to_index[dense]]); } // Construct the Ab matrix - fuse_core::MatrixXd Ab = - fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); - for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index) { - const auto & linear_term = linear_terms[term_index]; + fuse_core::MatrixXd Ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); + for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index) + { + const auto& linear_term = linear_terms[term_index]; auto row_offset = row_offsets[term_index]; - for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { - const auto & A = linear_term.A[i]; + for (size_t i = 0ul; i < linear_term.variables.size(); ++i) + { + const auto& A = linear_term.A[i]; auto dense = index_to_dense[linear_term.variables[i]]; auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) { + for (int row = 0; row < A.rows(); ++row) + { + for (int col = 0; col < A.cols(); ++col) + { Ab(row_offset + row, column_offset + col) = A(row, col); } } } - const auto & b = linear_term.b; + const auto& b = linear_term.b; int column_offset = column_offsets.back(); - for (int row = 0; row < b.rows(); ++row) { + for (int row = 0; row < b.rows(); ++row) + { Ab(row_offset + row, column_offset) = b(row); } } @@ -492,9 +513,7 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto size = std::min(rows, cols); auto hCoeffs = HCoeffsType(size); auto temp = RowVectorType(cols); - Eigen::internal::householder_qr_inplace_blocked::run( - Ab, hCoeffs, 48, - temp.data()); + Eigen::internal::householder_qr_inplace_blocked::run(Ab, hCoeffs, 48, temp.data()); Ab.triangularView().setZero(); // Zero out the below-diagonal elements } @@ -506,18 +525,22 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto max_row = std::min(Ab.rows(), Ab.cols() - 1); // -1 for the included b vector auto marginal_rows = max_row - min_row; auto marginal_term = LinearTerm(); - if (marginal_rows > 0) { + if (marginal_rows > 0) + { auto variable_count = dense_to_index.size() - 1; marginal_term.variables.reserve(variable_count); marginal_term.A.reserve(variable_count); // Skipping the marginalized variable - for (size_t dense = 1ul; dense < dense_to_index.size(); ++dense) { + for (size_t dense = 1ul; dense < dense_to_index.size(); ++dense) + { auto index = dense_to_index[dense]; marginal_term.variables.push_back(index); fuse_core::MatrixXd A = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]); auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) { + for (int row = 0; row < A.rows(); ++row) + { + for (int col = 0; col < A.cols(); ++col) + { A(row, col) = Ab(min_row + row, column_offset + col); } } @@ -525,32 +548,26 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) } marginal_term.b = fuse_core::VectorXd::Zero(marginal_rows); auto column_offset = column_offsets.back(); - for (int row = 0; row < marginal_term.b.rows(); ++row) { + for (int row = 0; row < marginal_term.b.rows(); ++row) + { marginal_term.b(row) = Ab(min_row + row, column_offset); } } return marginal_term; } -MarginalConstraint::SharedPtr createMarginalConstraint( - const std::string & source, - const LinearTerm & linear_term, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order) +MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, + const fuse_core::Graph& graph, + const UuidOrdering& elimination_order) { - auto index_to_variable = - [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable & - { - return graph.getVariable(elimination_order.at(index)); - }; + auto index_to_variable = [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable& { + return graph.getVariable(elimination_order.at(index)); + }; return MarginalConstraint::make_shared( - source, - boost::make_transform_iterator(linear_term.variables.begin(), index_to_variable), - boost::make_transform_iterator(linear_term.variables.end(), index_to_variable), - linear_term.A.begin(), - linear_term.A.end(), - linear_term.b); + source, boost::make_transform_iterator(linear_term.variables.begin(), index_to_variable), + boost::make_transform_iterator(linear_term.variables.end(), index_to_variable), linear_term.A.begin(), + linear_term.A.end(), linear_term.b); } } // namespace detail diff --git a/fuse_constraints/src/normal_delta.cpp b/fuse_constraints/src/normal_delta.cpp index 50b70b33c..469064043 100644 --- a/fuse_constraints/src/normal_delta.cpp +++ b/fuse_constraints/src/normal_delta.cpp @@ -39,9 +39,7 @@ namespace fuse_constraints { -NormalDelta::NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorXd & b) -: A_(A), - b_(b) +NormalDelta::NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b) : A_(A), b_(b) { CHECK_GT(b_.rows(), 0); CHECK_GT(A_.rows(), 0); @@ -51,25 +49,21 @@ NormalDelta::NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorX mutable_parameter_block_sizes()->push_back(b_.rows()); } -bool NormalDelta::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDelta::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { Eigen::Map x0(parameters[0], parameter_block_sizes()[0]); Eigen::Map x1(parameters[1], parameter_block_sizes()[1]); Eigen::Map r(residuals, num_residuals()); r = A_ * (x1 - x0 - b_); - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { - Eigen::Map( - jacobians[0], num_residuals(), - parameter_block_sizes()[0]) = -A_; + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { + Eigen::Map(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = -A_; } - if (jacobians[1] != nullptr) { - Eigen::Map( - jacobians[1], num_residuals(), - parameter_block_sizes()[1]) = A_; + if (jacobians[1] != nullptr) + { + Eigen::Map(jacobians[1], num_residuals(), parameter_block_sizes()[1]) = A_; } } return true; diff --git a/fuse_constraints/src/normal_delta_orientation_2d.cpp b/fuse_constraints/src/normal_delta_orientation_2d.cpp index b90625992..b78960b1e 100644 --- a/fuse_constraints/src/normal_delta_orientation_2d.cpp +++ b/fuse_constraints/src/normal_delta_orientation_2d.cpp @@ -37,27 +37,25 @@ namespace fuse_constraints { -NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) -: A_(A), - b_(b) +NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) : A_(A), b_(b) { } -bool NormalDeltaOrientation2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDeltaOrientation2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // The following lines should read as // r = A_ * ((x1 - x0) - b_); // The wrap function handles the 2_pi wrap around issue with rotations double angle_diff = fuse_core::wrapAngle2D(parameters[1][0] - parameters[0][0] - b_); residuals[0] = A_ * angle_diff; - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { jacobians[0][0] = -A_; } - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { jacobians[1][0] = A_; } } diff --git a/fuse_constraints/src/normal_delta_pose_2d.cpp b/fuse_constraints/src/normal_delta_pose_2d.cpp index 93677dbe4..66a0712e7 100644 --- a/fuse_constraints/src/normal_delta_pose_2d.cpp +++ b/fuse_constraints/src/normal_delta_pose_2d.cpp @@ -40,60 +40,53 @@ namespace fuse_constraints { -NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); set_num_residuals(A_.rows()); } -bool NormalDeltaPose2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDeltaPose2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - const fuse_core::Matrix2d R1_transpose = - fuse_core::rotationMatrix2D(parameters[1][0]).transpose(); // orientation1 + const fuse_core::Matrix2d R1_transpose = fuse_core::rotationMatrix2D(parameters[1][0]).transpose(); // orientation1 const fuse_core::Vector2d position_delta = - R1_transpose * fuse_core::Vector2d( - parameters[2][0] - parameters[0][0], // position2.x - position1.x - parameters[2][1] - parameters[0][1]); // position2.y - position1.y + R1_transpose * fuse_core::Vector2d(parameters[2][0] - parameters[0][0], // position2.x - position1.x + parameters[2][1] - parameters[0][1]); // position2.y - position1.y - const fuse_core::Vector3d full_residuals_vector( - position_delta[0] - b_[0], position_delta[1] - b_[1], - // orientation2 - orientation1 - fuse_core::wrapAngle2D(parameters[3][0] - parameters[1][0] - b_[2])); + const fuse_core::Vector3d full_residuals_vector(position_delta[0] - b_[0], position_delta[1] - b_[1], + // orientation2 - orientation1 + fuse_core::wrapAngle2D(parameters[3][0] - parameters[1][0] - b_[2])); // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_vector(residuals, num_residuals()); residuals_vector = A_ * full_residuals_vector; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position1 - if (jacobians[0] != nullptr) { - Eigen::Map( - jacobians[0], num_residuals(), - 2) = -A_.leftCols<2>() * R1_transpose; + if (jacobians[0] != nullptr) + { + Eigen::Map(jacobians[0], num_residuals(), 2) = -A_.leftCols<2>() * R1_transpose; } // Jacobian wrt orientation1 - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map(jacobians[1], num_residuals()) = - A_ * fuse_core::Vector3d(position_delta[1], -position_delta[0], -1); + A_ * fuse_core::Vector3d(position_delta[1], -position_delta[0], -1); } // Jacobian wrt position2 - if (jacobians[2] != nullptr) { - Eigen::Map( - jacobians[2], num_residuals(), - 2) = A_.leftCols<2>() * R1_transpose; + if (jacobians[2] != nullptr) + { + Eigen::Map(jacobians[2], num_residuals(), 2) = A_.leftCols<2>() * R1_transpose; } // Jacobian wrt orientation2 - if (jacobians[3] != nullptr) { + if (jacobians[3] != nullptr) + { Eigen::Map(jacobians[3], num_residuals()) = A_.col(2); } } diff --git a/fuse_constraints/src/normal_prior_orientation_2d.cpp b/fuse_constraints/src/normal_prior_orientation_2d.cpp index 6d5116bfb..faff1ad24 100644 --- a/fuse_constraints/src/normal_prior_orientation_2d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_2d.cpp @@ -37,23 +37,19 @@ namespace fuse_constraints { -NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) -: A_(A), - b_(b) +NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) : A_(A), b_(b) { } -bool NormalPriorOrientation2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorOrientation2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // The following lines should read as // r = A_ * (x - b_); // The wrap function handles the 2_pi wrap around issue with rotations double angle_diff = fuse_core::wrapAngle2D(parameters[0][0] - b_); residuals[0] = A_ * angle_diff; - if ((jacobians != nullptr) && (jacobians[0] != nullptr)) { + if ((jacobians != nullptr) && (jacobians[0] != nullptr)) + { jacobians[0][0] = A_; } return true; diff --git a/fuse_constraints/src/normal_prior_pose_2d.cpp b/fuse_constraints/src/normal_prior_pose_2d.cpp index 32af6b37a..76de289f1 100644 --- a/fuse_constraints/src/normal_prior_pose_2d.cpp +++ b/fuse_constraints/src/normal_prior_pose_2d.cpp @@ -40,23 +40,18 @@ namespace fuse_constraints { -NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); set_num_residuals(A_.rows()); } -bool NormalPriorPose2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { fuse_core::Vector3d full_residuals_vector; - full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x - full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y + full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x + full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y full_residuals_vector[2] = fuse_core::wrapAngle2D(parameters[1][0] - b_[2]); // orientation // Scale the residuals by the square root information matrix to account for the measurement @@ -64,14 +59,17 @@ bool NormalPriorPose2D::Evaluate( Eigen::Map residuals_vector(residuals, num_residuals()); residuals_vector = A_ * full_residuals_vector; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 2) = A_.leftCols<2>(); } // Jacobian wrt orientation - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map(jacobians[1], num_residuals()) = A_.col(2); } } diff --git a/fuse_constraints/src/relative_constraint.cpp b/fuse_constraints/src/relative_constraint.cpp index 4d55afb9c..ebfadfed5 100644 --- a/fuse_constraints/src/relative_constraint.cpp +++ b/fuse_constraints/src/relative_constraint.cpp @@ -43,24 +43,10 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePosition3DStampedConstrai BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityLinear2DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeOrientation2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativePosition2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativePosition3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeVelocityAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeVelocityLinear2DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityLinear2DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp index c7cbf31bd..11891f3be 100644 --- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp @@ -45,36 +45,29 @@ namespace fuse_constraints { RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector4d & delta, - const fuse_core::Matrix3d & covariance) -: fuse_core::Constraint(source, {orientation1.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector4d& delta, + const fuse_core::Matrix3d& covariance) + : fuse_core::Constraint(source, { orientation1.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const Eigen::Quaterniond & delta, - const fuse_core::Matrix3d & covariance) -: RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen( - delta), covariance) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const Eigen::Quaterniond& delta, + const fuse_core::Matrix3d& covariance) + : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), covariance) { } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const geometry_msgs::msg::Quaternion & delta, - const std::array & covariance) -: RelativeOrientation3DStampedConstraint( - source, orientation1, orientation2, toEigen(delta), toEigen(covariance)) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const geometry_msgs::msg::Quaternion& delta, + const std::array& covariance) + : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), toEigen(covariance)) { } @@ -83,7 +76,7 @@ fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::covariance() const return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void RelativeOrientation3DStampedConstraint::print(std::ostream & stream) const +void RelativeOrientation3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -93,37 +86,34 @@ void RelativeOrientation3DStampedConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * RelativeOrientation3DStampedConstraint::costFunction() const +ceres::CostFunction* RelativeOrientation3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_)); + new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_)); } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen( - const Eigen::Quaterniond & quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen( - const geometry_msgs::msg::Quaternion & quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen( - const std::array & covariance) +fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen(const std::array& covariance) { return fuse_core::Matrix3d(covariance.data()); } @@ -131,6 +121,4 @@ fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen( } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeOrientation3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeOrientation3DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp index 99150b987..dd968bee6 100644 --- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp @@ -45,18 +45,13 @@ namespace fuse_constraints { RelativePose2DStampedConstraint::RelativePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & orientation1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & orientation2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}) // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, + const std::vector& angular_indices) + : fuse_core::Constraint(source, + { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) // NOLINT { size_t total_variable_size = position1.size() + orientation1.size(); size_t total_indices = linear_indices.size() + angular_indices.size(); @@ -80,12 +75,14 @@ RelativePose2DStampedConstraint::RelativePose2DStampedConstraint( delta_ = fuse_core::Vector3d::Zero(); sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); - for (size_t i = 0; i < linear_indices.size(); ++i) { + for (size_t i = 0; i < linear_indices.size(); ++i) + { delta_(linear_indices[i]) = partial_delta(i); sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); } - for (size_t i = linear_indices.size(); i < total_indices; ++i) { + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { size_t final_index = position1.size() + angular_indices[i - linear_indices.size()]; delta_(final_index) = partial_delta(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); @@ -106,7 +103,7 @@ fuse_core::Matrix3d RelativePose2DStampedConstraint::covariance() const return pinv * pinv.transpose(); } -void RelativePose2DStampedConstraint::print(std::ostream & stream) const +void RelativePose2DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -118,13 +115,14 @@ void RelativePose2DStampedConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * RelativePose2DStampedConstraint::costFunction() const +ceres::CostFunction* RelativePose2DStampedConstraint::costFunction() const { return new NormalDeltaPose2D(sqrt_information_, delta_); } diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 7d627e117..f4ca63a16 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -44,22 +44,18 @@ namespace fuse_constraints { RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector7d& delta, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void RelativePose3DStampedConstraint::print(std::ostream & stream) const +void RelativePose3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -72,10 +68,10 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const +ceres::CostFunction* RelativePose3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/uuid_ordering.cpp b/fuse_constraints/src/uuid_ordering.cpp old mode 100755 new mode 100644 index 0608d17ff..baa9d89c7 --- a/fuse_constraints/src/uuid_ordering.cpp +++ b/fuse_constraints/src/uuid_ordering.cpp @@ -37,7 +37,7 @@ namespace fuse_constraints { UuidOrdering::UuidOrdering(std::initializer_list uuid_list) -: UuidOrdering(uuid_list.begin(), uuid_list.end()) + : UuidOrdering(uuid_list.begin(), uuid_list.end()) { } @@ -56,34 +56,34 @@ bool UuidOrdering::exists(const unsigned int index) const return index < order_.size(); } -bool UuidOrdering::exists(const fuse_core::UUID & uuid) const +bool UuidOrdering::exists(const fuse_core::UUID& uuid) const { return order_.right.find(uuid) != order_.right.end(); } -bool UuidOrdering::push_back(const fuse_core::UUID & uuid) +bool UuidOrdering::push_back(const fuse_core::UUID& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return result.second; } -const fuse_core::UUID & UuidOrdering::operator[](const unsigned int index) const +const fuse_core::UUID& UuidOrdering::operator[](const unsigned int index) const { return order_.left[index].second; } -unsigned int UuidOrdering::operator[](const fuse_core::UUID & uuid) +unsigned int UuidOrdering::operator[](const fuse_core::UUID& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return (*result.first).get_left(); } -const fuse_core::UUID & UuidOrdering::at(const unsigned int index) const +const fuse_core::UUID& UuidOrdering::at(const unsigned int index) const { return order_.left.at(index).second; } -unsigned int UuidOrdering::at(const fuse_core::UUID & uuid) const +unsigned int UuidOrdering::at(const fuse_core::UUID& uuid) const { return order_.right.at(uuid); } diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp old mode 100755 new mode 100644 index 8be6b3f49..4ee2fec67 --- a/fuse_constraints/src/variable_constraints.cpp +++ b/fuse_constraints/src/variable_constraints.cpp @@ -51,10 +51,7 @@ bool VariableConstraints::empty() const size_t VariableConstraints::size() const { - auto sum_edges = [](const size_t input, const ConstraintCollection & edges) - { - return input + edges.size(); - }; + auto sum_edges = [](const size_t input, const ConstraintCollection& edges) { return input + edges.size(); }; return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges); } @@ -65,33 +62,35 @@ unsigned int VariableConstraints::nextVariableIndex() const void VariableConstraints::insert(const unsigned int constraint, const unsigned int variable) { - if (variable >= variable_constraints_.size()) { + if (variable >= variable_constraints_.size()) + { variable_constraints_.resize(variable + 1); } variable_constraints_[variable].insert(constraint); } -void VariableConstraints::insert( - const unsigned int constraint, - std::initializer_list variable_list) +void VariableConstraints::insert(const unsigned int constraint, std::initializer_list variable_list) { return insert(constraint, variable_list.begin(), variable_list.end()); } void VariableConstraints::insert(const unsigned int variable) { - if (variable >= variable_constraints_.size()) { + if (variable >= variable_constraints_.size()) + { // This automatically create a new variable entry with an empty ConstraintCollection variable_constraints_.resize(variable + 1); } } -void VariableConstraints::print(std::ostream & stream) const +void VariableConstraints::print(std::ostream& stream) const { - for (size_t variable = 0; variable < variable_constraints_.size(); ++variable) { + for (size_t variable = 0; variable < variable_constraints_.size(); ++variable) + { stream << variable << ": ["; - for (const auto & constraint : variable_constraints_[variable]) { + for (const auto& constraint : variable_constraints_[variable]) + { stream << constraint << ", "; } @@ -99,7 +98,7 @@ void VariableConstraints::print(std::ostream & stream) const } } -std::ostream & operator<<(std::ostream & stream, const VariableConstraints & variable_constraints) +std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints) { variable_constraints.print(stream); return stream; diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 3687d0504..e77f593e0 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -50,25 +50,25 @@ * @param[in] cost_function The expected cost function * @param[in] actual_cost_function The actual cost function */ -static void ExpectCostFunctionsAreEqual( - const ceres::CostFunction & cost_function, - const ceres::CostFunction & actual_cost_function) +static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function, + const ceres::CostFunction& actual_cost_function) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); - const std::vector & parameter_block_sizes = cost_function.parameter_block_sizes(); - const std::vector & actual_parameter_block_sizes = - actual_cost_function.parameter_block_sizes(); + const std::vector& parameter_block_sizes = cost_function.parameter_block_sizes(); + const std::vector& actual_parameter_block_sizes = actual_cost_function.parameter_block_sizes(); EXPECT_EQ(parameter_block_sizes.size(), actual_parameter_block_sizes.size()); size_t num_parameters = 0; - for (size_t i = 0; i < parameter_block_sizes.size(); ++i) { + for (size_t i = 0; i < parameter_block_sizes.size(); ++i) + { EXPECT_EQ(parameter_block_sizes[i], actual_parameter_block_sizes[i]); num_parameters += parameter_block_sizes[i]; } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) { + for (size_t i = 0; i < num_parameters; ++i) + { parameters[i] = static_cast(i) + 1.0; } @@ -78,12 +78,13 @@ static void ExpectCostFunctionsAreEqual( std::unique_ptr actual_residuals(new double[num_residuals]); std::unique_ptr actual_jacobians(new double[num_parameters * num_residuals]); - std::unique_ptr parameter_blocks(new double *[parameter_block_sizes.size()]); - std::unique_ptr jacobian_blocks(new double *[parameter_block_sizes.size()]); - std::unique_ptr actual_jacobian_blocks(new double *[parameter_block_sizes.size()]); + std::unique_ptr parameter_blocks(new double*[parameter_block_sizes.size()]); + std::unique_ptr jacobian_blocks(new double*[parameter_block_sizes.size()]); + std::unique_ptr actual_jacobian_blocks(new double*[parameter_block_sizes.size()]); num_parameters = 0; - for (size_t i = 0; i < parameter_block_sizes.size(); ++i) { + for (size_t i = 0; i < parameter_block_sizes.size(); ++i) + { parameter_blocks[i] = parameters.get() + num_parameters; jacobian_blocks[i] = jacobians.get() + num_parameters * num_residuals; actual_jacobian_blocks[i] = actual_jacobians.get() + num_parameters * num_residuals; @@ -91,29 +92,24 @@ static void ExpectCostFunctionsAreEqual( } EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), nullptr)); - EXPECT_TRUE( - actual_cost_function.Evaluate( - parameter_blocks.get(), actual_residuals.get(), - nullptr)); - for (size_t i = 0; i < num_residuals; ++i) { + EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), nullptr)); + for (size_t i = 0; i < num_residuals; ++i) + { EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; } + EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), jacobian_blocks.get())); EXPECT_TRUE( - cost_function.Evaluate( - parameter_blocks.get(), residuals.get(), - jacobian_blocks.get())); - EXPECT_TRUE( - actual_cost_function.Evaluate( - parameter_blocks.get(), actual_residuals.get(), - actual_jacobian_blocks.get())); - for (size_t i = 0; i < num_residuals; ++i) { + actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); + for (size_t i = 0; i < num_residuals; ++i) + { EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; } - for (size_t i = 0; i < num_residuals * num_parameters; ++i) { + for (size_t i = 0; i < num_residuals * num_parameters; ++i) + { EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) - << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; + << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index a9f7921e6..18583aab0 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -57,97 +57,76 @@ TEST(AbsoluteConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. { - fuse_variables::AccelerationAngular2DStamped variable( - rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bender")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("johnny5")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); 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(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("rosie")); + fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("rosie")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); 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(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("clank")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("clank")); fuse_core::VectorXd mean(3); mean << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov(3, 3); 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(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("gort")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bishop")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW( - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint( - "test", variable, mean, - cov)); + EXPECT_NO_THROW(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint("test", variable, mean, cov)); } } TEST(AbsoluteConstraint, PartialMeasurement) { - fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("vici")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("vici")); fuse_core::VectorXd mean(2); mean << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); 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)); + fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices)); } TEST(AbsoluteConstraint, Covariance) @@ -156,18 +135,16 @@ TEST(AbsoluteConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("chappie")); + fuse_core::uuid::generate("chappie")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, - mean, cov); + 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; /* *INDENT-OFF* */ - expected_sqrt_info << 1.002509414234171, -0.050125470711709, - 0.000000000000000, 0.707106781186547; + expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547; /* *INDENT-ON* */ fuse_core::Matrix2d expected_cov = cov; // Compare @@ -176,15 +153,13 @@ TEST(AbsoluteConstraint, Covariance) } // Test the covariance of a partial measurement { - fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("astroboy")); fuse_core::VectorXd mean(2); mean << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, - indices); + auto indices = std::vector{ 2, 0 }; + fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices); // Define the expected matrices fuse_core::Vector3d expected_mean; expected_mean << 1.0, 0.0, 3.0; @@ -192,8 +167,8 @@ TEST(AbsoluteConstraint, Covariance) 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); /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ // Compare EXPECT_TRUE(expected_mean.isApprox(constraint.mean(), 1.0e-9)); @@ -208,9 +183,8 @@ TEST(AbsoluteConstraint, Optimization) { // Optimize a single variable and single constraint, verify the expected value and covariance // are generated. Create a variable - auto variable = fuse_variables::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto variable = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("t800")); variable->x() = 10.7; variable->y() = -3.2; // Create an absolute constraint @@ -218,29 +192,21 @@ TEST(AbsoluteConstraint, Optimization) mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - variable->data(), - variable->size(), + problem.AddParameterBlock(variable->data(), variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - variable->localParameterization()); + variable->localParameterization()); #else - variable->manifold()); + variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -249,7 +215,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); @@ -266,8 +232,7 @@ TEST(AbsoluteConstraint, Optimization) // Optimize a single variable with a full measurement and a partial measurement // Verify the expected value and covariance are generated. // Create a variable - auto var = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); + auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); var->x() = 10.7; var->y() = -3.2; var->z() = 0.9; @@ -276,47 +241,32 @@ TEST(AbsoluteConstraint, Optimization) mean1 << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov1; /* *INDENT-OFF* */ - cov1 << 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0; + cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", - *var, - mean1, - cov1); + auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1); fuse_core::Vector2d mean2; mean2 << 4.0, 2.0; fuse_core::Matrix2d cov2; /* *INDENT-OFF* */ - cov2 << 1.0, 0.0, - 0.0, 1.0; + cov2 << 1.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - auto indices2 = std::vector{2, 0}; - auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", *var, mean2, cov2, indices2); + 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - var->data(), - var->size(), + problem.AddParameterBlock(var->data(), var->size(), #if !CERES_SUPPORTS_MANIFOLDS - var->localParameterization()); + var->localParameterization()); #else - var->manifold()); + var->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -326,7 +276,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); @@ -336,9 +286,7 @@ TEST(AbsoluteConstraint, Optimization) fuse_core::Matrix3d actual_cov(covariance_vector.data()); fuse_core::Matrix3d expected_cov; /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ EXPECT_TRUE(expected_cov.isApprox(actual_cov, 1.0e-9)); } @@ -347,8 +295,7 @@ TEST(AbsoluteConstraint, Optimization) TEST(AbsoluteConstraint, PartialOptimization) { // Create a variable - auto var = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); + auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); var->x() = 10.7; var->y() = -3.2; var->z() = 0.9; @@ -358,45 +305,35 @@ TEST(AbsoluteConstraint, PartialOptimization) mean1 << 1.0, 3.0; fuse_core::MatrixXd cov1(2, 2); /* *INDENT-OFF* */ - cov1 << 1.0, 0.0, - 0.0, 1.0; + cov1 << 1.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - std::vector indices1 = - {fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X}; - auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", *var, mean1, cov1, indices1); + 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::VectorXd mean2(1); mean2 << 2.0; fuse_core::MatrixXd cov2(1, 1); 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; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - var->data(), - var->size(), + problem.AddParameterBlock(var->data(), var->size(), #if !CERES_SUPPORTS_MANIFOLDS - var->localParameterization()); + var->localParameterization()); #else - var->manifold()); + var->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -411,35 +348,28 @@ 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( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("tiktok")); + auto variable = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("tiktok")); variable->yaw() = 0.7; // Create an absolute constraint fuse_core::VectorXd mean(1); mean << 7.0; fuse_core::MatrixXd cov(1, 1); 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - variable->data(), - variable->size(), + problem.AddParameterBlock(variable->data(), variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - variable->localParameterization()); + variable->localParameterization()); #else - variable->manifold()); + variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -447,7 +377,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) // Check EXPECT_NEAR(7.0 - 2 * M_PI, variable->yaw(), 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); @@ -461,14 +391,12 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) TEST(AbsoluteConstraint, Serialization) { // Construct a constraint - fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean, - cov); + fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; 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 b745c8cc5..520ebb5f9 100644 --- a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -52,27 +52,21 @@ using fuse_constraints::AbsoluteOrientation2DStampedConstraint; using fuse_variables::Orientation2DStamped; - TEST(AbsoluteOrientation2DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - AbsoluteOrientation2DStampedConstraint constraint( - "test", orientation_variable, - mean, cov)); + EXPECT_NO_THROW(AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov)); } TEST(AbsoluteOrientation2DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); @@ -94,8 +88,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 1.0; // Create an absolute orientation constraint @@ -103,31 +96,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) mean << 1.0; fuse_core::MatrixXd cov(1, 1); 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; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -139,15 +123,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) EXPECT_NEAR(1.0, orientation_variable->yaw(), 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()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -161,9 +144,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) // covariance are generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), - fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.0; // Create an absolute orientation constraint @@ -171,31 +152,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) mean << 0.0; fuse_core::MatrixXd cov(1, 1); 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; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -207,15 +179,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) EXPECT_NEAR(0.0, orientation_variable->yaw(), 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()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -300,9 +271,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) // covariance are generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), - fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = -M_PI; // Create an absolute orientation constraint @@ -310,31 +279,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) mean << -M_PI; fuse_core::MatrixXd cov(1, 1); 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; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -346,15 +306,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) EXPECT_NEAR(-M_PI, orientation_variable->yaw(), 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()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -365,8 +324,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) TEST(AbsoluteOrientation2DStampedConstraint, Serialization) { // Construct a constraint - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp index 0a10bbbb5..dade370af 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp @@ -52,41 +52,29 @@ using fuse_constraints::AbsoluteOrientation3DStampedConstraint; using fuse_variables::Orientation3DStamped; - TEST(AbsoluteOrientation3DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.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( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - mean, cov)); + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, mean, cov)); Eigen::Quaterniond quat_eigen(1.0, 0.0, 0.0, 0.0); - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - quat_eigen, cov)); + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, quat_eigen, cov)); geometry_msgs::msg::Quaternion quat_geom; quat_geom.w = 1.0; - std::array cov_arr = {1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0}; - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - quat_geom, cov_arr)); + std::array cov_arr = { 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0 }; + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, quat_geom, cov_arr)); } TEST(AbsoluteOrientation3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; @@ -96,9 +84,8 @@ TEST(AbsoluteOrientation3DStampedConstraint, Covariance) // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; @@ -111,8 +98,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -123,35 +109,23 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; - cov << - 1.0, 0.1, 0.2, - 0.1, 2.0, 0.3, - 0.2, 0.3, 3.0; - auto constraint = AbsoluteOrientation3DStampedConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov); + cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; + auto constraint = AbsoluteOrientation3DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -165,30 +139,25 @@ TEST(AbsoluteOrientation3DStampedConstraint, 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(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); - fuse_core::Matrix3d actual_covariance(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + fuse_core::Matrix3d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix3d expected_covariance; - expected_covariance << - 1.0, 0.1, 0.2, - 0.1, 2.0, 0.3, - 0.2, 0.3, 3.0; + expected_covariance << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); } TEST(AbsoluteOrientation3DStampedConstraint, Serialization) { // Construct a constraint - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 9787d83f6..3929b84ce 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -50,46 +50,36 @@ using fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint; using fuse_variables::Orientation3DStamped; - TEST(AbsoluteOrientation3DStampedEulerConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; 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; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedEulerConstraint constraint( - "test", orientation_variable, mean, cov, - axes)); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + EXPECT_NO_THROW(AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes)); } TEST(AbsoluteOrientation3DStampedEulerConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector3d mean; 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; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; @@ -102,8 +92,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -114,35 +103,25 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) mean << 0.5, 1.0, 1.5; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - auto constraint = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + auto constraint = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean, cov, axes); // 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(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -151,8 +130,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) // Check Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3); EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3); @@ -165,8 +144,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -177,51 +155,34 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) mean1 << 0.5, 1.5; fuse_core::Matrix2d cov1; cov1 << 1.0, 0.2, 0.2, 3.0; - std::vector axes1 = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::PITCH}; - auto constraint1 = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean1, - cov1, - axes1); + std::vector axes1 = { Orientation3DStamped::Euler::YAW, + Orientation3DStamped::Euler::PITCH }; + auto constraint1 = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean1, cov1, axes1); fuse_core::Vector1d mean2; mean2 << 1.0; fuse_core::Matrix1d cov2; cov2 << 2.0; - std::vector axes2 = - {Orientation3DStamped::Euler::ROLL}; - auto constraint2 = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean2, - cov2, - axes2); + std::vector axes2 = { Orientation3DStamped::Euler::ROLL }; + auto constraint2 = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean2, cov2, axes2); // 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(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->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; @@ -230,8 +191,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) // Check Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3); EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3); @@ -243,17 +204,14 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization) { // Construct a constraint - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; 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; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - AbsoluteOrientation3DStampedEulerConstraint expected("test", orientation_variable, mean, cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + AbsoluteOrientation3DStampedEulerConstraint expected("test", orientation_variable, mean, cov, axes); // Serialize the constraint into an archive std::stringstream stream; 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 ef88fe0dd..c8092ce36 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -48,45 +48,38 @@ #include #include +using fuse_constraints::AbsolutePose2DStampedConstraint; using fuse_variables::Orientation2DStamped; using fuse_variables::Position2DStamped; -using fuse_constraints::AbsolutePose2DStampedConstraint; - TEST(AbsolutePose2DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; 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( - AbsolutePose2DStampedConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose2DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector3d mean; 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; - AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; // Compare @@ -98,11 +91,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.8; - auto position_variable = Position2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; // Create an absolute pose constraint @@ -110,39 +101,28 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) 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; - auto constraint = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + auto constraint = + AbsolutePose2DStampedConstraint::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(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - std::vector parameter_blocks; + 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; ceres::Solver::Summary summary; @@ -152,7 +132,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // 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(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); @@ -160,18 +140,11 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); std::vector covariance_vector1(position_variable->size() * position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), covariance_vector1.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), covariance_vector1.data()); std::vector covariance_vector2(position_variable->size() * orientation_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - orientation_variable->data(), covariance_vector2.data()); - std::vector covariance_vector3(orientation_variable->size() * - orientation_variable->size()); - covariance.GetCovarianceBlock( - orientation_variable->data(), - orientation_variable->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(position_variable->data(), orientation_variable->data(), covariance_vector2.data()); + std::vector covariance_vector3(orientation_variable->size() * orientation_variable->size()); + covariance.GetCovarianceBlock(orientation_variable->data(), orientation_variable->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -191,11 +164,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.8; - auto position_variable = Position2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; @@ -204,64 +175,42 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) mean1 << 1.0, 3.0; fuse_core::Matrix2d cov1; cov1 << 1.0, 0.2, 0.2, 3.0; - std::vector axes_lin1 = {fuse_variables::Position2DStamped::X}; - std::vector axes_ang1 = {fuse_variables::Orientation2DStamped::YAW}; - auto constraint1 = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean1, - cov1, - axes_lin1, - axes_ang1); + std::vector axes_lin1 = { fuse_variables::Position2DStamped::X }; + std::vector axes_ang1 = { fuse_variables::Orientation2DStamped::YAW }; + auto constraint1 = AbsolutePose2DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, + mean1, cov1, axes_lin1, axes_ang1); // Create an absolute pose constraint fuse_core::Vector1d mean2; mean2 << 2.0; fuse_core::Matrix1d cov2; cov2 << 2.0; - std::vector axes_lin2 = {fuse_variables::Position2DStamped::Y}; + std::vector axes_lin2 = { fuse_variables::Position2DStamped::Y }; std::vector axes_ang2; - auto constraint2 = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean2, - cov2, - axes_lin2, - axes_ang2); + auto constraint2 = AbsolutePose2DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, + mean2, cov2, axes_lin2, axes_ang2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->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; @@ -274,7 +223,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // 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(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); @@ -282,18 +231,11 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); std::vector covariance_vector1(position_variable->size() * position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), covariance_vector1.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), covariance_vector1.data()); std::vector covariance_vector2(position_variable->size() * orientation_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - orientation_variable->data(), covariance_vector2.data()); - std::vector covariance_vector3(orientation_variable->size() * - orientation_variable->size()); - covariance.GetCovarianceBlock( - orientation_variable->data(), - orientation_variable->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(position_variable->data(), orientation_variable->data(), covariance_vector2.data()); + std::vector covariance_vector3(orientation_variable->size() * orientation_variable->size()); + covariance.GetCovarianceBlock(orientation_variable->data(), orientation_variable->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -322,15 +264,13 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) TEST(AbsolutePose2DStampedConstraint, Serialization) { // Construct a constraint - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; 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; - AbsolutePose2DStampedConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose2DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; 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 e13c14186..e7d93e9cc 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -48,17 +48,15 @@ #include #include +using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; -using fuse_constraints::AbsolutePose3DStampedConstraint; - TEST(AbsolutePose3DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -67,26 +65,29 @@ TEST(AbsolutePose3DStampedConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ EXPECT_NO_THROW( - AbsolutePose3DStampedConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -95,26 +96,32 @@ TEST(AbsolutePose3DStampedConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + 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; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -127,14 +134,12 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; position_variable->z() = 10.0; - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -146,50 +151,35 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, 0.5, 0.2, 4.0, + 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + 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( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + 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; @@ -206,7 +196,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()); @@ -215,18 +205,15 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), cov_pos_pos.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), cov_pos_pos.data()); - fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position_variable->data(), orientation_variable->data(), + cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -235,13 +222,8 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + expected_covariance << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, + 0.5, 0.2, 4.0, 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); @@ -251,8 +233,7 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) { // Construct a constraint Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -261,16 +242,21 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_marginal_constraint.cpp b/fuse_constraints/test/test_marginal_constraint.cpp index cf49f9ef0..58409d5f0 100644 --- a/fuse_constraints/test/test_marginal_constraint.cpp +++ b/fuse_constraints/test/test_marginal_constraint.cpp @@ -64,13 +64,8 @@ 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(); @@ -79,10 +74,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 @@ -123,13 +118,8 @@ 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(); @@ -141,11 +131,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 @@ -184,13 +174,8 @@ 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 @@ -201,14 +186,11 @@ 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()}; - cost_function->Evaluate( - variable_values.data(), - actual_residuals.data(), - actual_jacobians.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 fuse_core::Vector1d expected_residuals; @@ -245,13 +227,8 @@ 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; @@ -277,15 +254,16 @@ TEST(MarginalConstraint, Serialization) #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) { - auto actual_derived = std::dynamic_pointer_cast( - actual.localParameterizations()[i]); + for (auto i = 0u; i < actual.localParameterizations().size(); ++i) + { + auto actual_derived = std::dynamic_pointer_cast(actual.localParameterizations()[i]); EXPECT_TRUE(static_cast(actual_derived)); } #else using ExpectedManifold = fuse_variables::Orientation3DManifold; ASSERT_EQ(expected.manifolds().size(), actual.manifolds().size()); - for (auto i = 0u; i < actual.manifolds().size(); ++i) { + 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)); } @@ -314,13 +292,8 @@ TEST(MarginalConstraint, LegacyDeserialization) 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); // The legacy serialization file was generated using the following code: // { @@ -346,7 +319,8 @@ TEST(MarginalConstraint, LegacyDeserialization) // 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) { + 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)); } diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 591fc6d65..2530a887b 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -63,22 +63,31 @@ class GenericVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(GenericVariable) - GenericVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_{} - {} + GenericVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} + { + } - explicit GenericVariable(const fuse_core::UUID & uuid) - : fuse_core::Variable(uuid), - data_{} - {} + explicit GenericVariable(const fuse_core::UUID& uuid) : fuse_core::Variable(uuid), data_{} + { + } - size_t size() const override {return 1;} + size_t size() const override + { + return 1; + } - const double * data() const override {return &data_;} - double * data() override {return &data_;} + const double* data() const override + { + return &data_; + } + double* data() override + { + return &data_; + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } protected: double data_; @@ -94,11 +103,11 @@ class GenericVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -112,18 +121,27 @@ class GenericConstraint : public fuse_core::Constraint public: FUSE_CONSTRAINT_DEFINITIONS(GenericConstraint) - GenericConstraint(std::initializer_list variable_uuids) - : Constraint("test", variable_uuids) {} + GenericConstraint(std::initializer_list variable_uuids) : Constraint("test", variable_uuids) + { + } - explicit GenericConstraint(const fuse_core::UUID & variable1) - : fuse_core::Constraint("test", {variable1}) {} + explicit GenericConstraint(const fuse_core::UUID& variable1) : fuse_core::Constraint("test", { variable1 }) + { + } - GenericConstraint(const fuse_core::UUID & variable1, const fuse_core::UUID & variable2) - : fuse_core::Constraint("test", {variable1, variable2}) {} + GenericConstraint(const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint("test", { variable1, variable2 }) + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::CostFunction * costFunction() const override {return nullptr;} + ceres::CostFunction* costFunction() const override + { + return nullptr; + } private: // Allow Boost Serialization access to private methods @@ -136,10 +154,10 @@ class GenericConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -150,10 +168,8 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped FixedOrientation3DStamped() = default; - explicit FixedOrientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : Orientation3DStamped(stamp, device_id) + explicit FixedOrientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : Orientation3DStamped(stamp, device_id) { } @@ -173,10 +189,10 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -208,7 +224,7 @@ TEST(MarginalizeVariables, ComputeEliminationOrder) graph.addConstraint(c6); // Define the set of variables to be marginalized - auto to_be_marginalized = std::vector{x2->uuid(), x1->uuid()}; + auto to_be_marginalized = std::vector{ x2->uuid(), x1->uuid() }; // Compute the ordering auto actual = fuse_constraints::computeEliminationOrder(to_be_marginalized, graph); @@ -222,7 +238,8 @@ TEST(MarginalizeVariables, ComputeEliminationOrder) // Check ASSERT_EQ(expected.size(), actual.size()); - for (size_t i = 0; i < expected.size(); ++i) { + for (size_t i = 0; i < expected.size(); ++i) + { SCOPED_TRACE(i); EXPECT_EQ(fuse_core::uuid::to_string(expected.at(i)), fuse_core::uuid::to_string(actual.at(i))); } @@ -259,14 +276,11 @@ TEST(MarginalizeVariables, ComputeEliminationOrderWithOrphanVariables) // fail) With 1 or 2 orphan variables computeEliminationOrder throws an std::runtime_error // exception because CCOLAMD fails With 3 or more orphan variables computeEliminationOrder crashes // with `free(): invalid next size (fast)` - auto o1 = - GenericVariable::make_shared( - fuse_core::uuid::from_string( - "b726fbef-4015-4dc8-b4dd-cb57d4439c74")); + auto o1 = GenericVariable::make_shared(fuse_core::uuid::from_string("b726fbef-4015-4dc8-b4dd-cb57d4439c74")); graph.addVariable(o1); // Define the set of variables to be marginalized - auto to_be_marginalized = std::vector{x2->uuid(), x1->uuid(), o1->uuid()}; + auto to_be_marginalized = std::vector{ x2->uuid(), x1->uuid(), o1->uuid() }; // Compute the ordering fuse_constraints::UuidOrdering actual; @@ -282,18 +296,21 @@ TEST(MarginalizeVariables, ComputeEliminationOrderWithOrphanVariables) // Check ASSERT_EQ(expected.size(), actual.size()); - for (size_t i = 0; i < expected.size(); ++i) { + for (size_t i = 0; i < expected.size(); ++i) + { SCOPED_TRACE(i); EXPECT_EQ(fuse_core::uuid::to_string(expected.at(i)), fuse_core::uuid::to_string(actual.at(i))); } // Check all marginalized variables are in the elimination order // This check is equivalent to the assert in marginalizeVariables - for (const auto & variable_uuid : to_be_marginalized) { + for (const auto& variable_uuid : to_be_marginalized) + { SCOPED_TRACE(fuse_core::uuid::to_string(variable_uuid)); EXPECT_TRUE(actual.exists(variable_uuid)); - if (actual.exists(variable_uuid)) { + if (actual.exists(variable_uuid)) + { EXPECT_GT(to_be_marginalized.size(), actual.at(variable_uuid)); } } @@ -318,11 +335,7 @@ TEST(MarginalizeVariables, Linearize) delta << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov; cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto constraint = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", - *x1, *x2, - delta, - cov); + auto constraint = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta, cov); auto graph = fuse_graphs::HashGraph(); graph.addVariable(x1); @@ -344,16 +357,16 @@ TEST(MarginalizeVariables, Linearize) // Define the expected values fuse_core::MatrixXd expected_A0(3, 3); /* *INDENT-OFF* */ - expected_A0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, - 0.27712824947756498073, -0.65053818745824854020, -2.5352112979770691226e-07, - 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; + expected_A0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + -0.65053818745824854020, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, + -0.57735026918958520792; /* *INDENT-ON* */ fuse_core::MatrixXd expected_A1(3, 3); /* *INDENT-OFF* */ - expected_A1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, - 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, - 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; + expected_A1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, + 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, + 2.0699913566568639567e-07, 0.57735026918958520792; /* *INDENT-ON* */ fuse_core::VectorXd expected_b(3); @@ -376,9 +389,9 @@ TEST(MarginalizeVariables, MarginalizeNext) term1.variables.push_back(1); auto A1 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, - -3.181980062078038074e-08, 0.70710678118654701763, 1.0606600528428877794e-08, - 1.7320505793505525105e-08, -8.6602525498080673572e-09, 0.57735026918962550901; + A1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, -3.181980062078038074e-08, + 0.70710678118654701763, 1.0606600528428877794e-08, 1.7320505793505525105e-08, -8.6602525498080673572e-09, + 0.57735026918962550901; /* *INDENT-ON* */ term1.A.push_back(A1); auto b1 = fuse_core::VectorXd(3); @@ -390,15 +403,15 @@ TEST(MarginalizeVariables, MarginalizeNext) term2.variables.push_back(1); auto A21 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, - 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, - 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; + A21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, + 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, 2.0699913566568639567e-07, + 0.57735026918958520792; /* *INDENT-ON* */ auto A22 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, - 0.27712824947756498073, -0.6505381874582485402, -2.5352112979770691226e-07, - 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; + A22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + -0.6505381874582485402, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, + -0.57735026918958520792; /* *INDENT-ON* */ term2.A.push_back(A21); term2.A.push_back(A22); @@ -418,9 +431,8 @@ TEST(MarginalizeVariables, MarginalizeNext) expected.variables.push_back(3); auto A_expected = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, - 0.000000000000000, -0.509885650799691, 0.000000079984512, - 0.000000000000000, 0.000000000000000, 0.408248290463911; + A_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, 0.000000000000000, -0.509885650799691, + 0.000000079984512, 0.000000000000000, 0.000000000000000, 0.408248290463911; /* *INDENT-ON* */ expected.A.push_back(A_expected); auto b_expected = fuse_core::VectorXd(3); @@ -465,29 +477,28 @@ TEST(MarginalizeVariables, MarginalizeVariables) mean1 << 0.92736185, 0.1, 0.2, 0.3; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *x1, mean1, cov1); + auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *x1, mean1, cov1); fuse_core::Vector4d delta2; delta2 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov2; cov2 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_x2 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *x2, delta2, cov2); + auto relative_x1_x2 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta2, cov2); fuse_core::Vector4d delta3; delta3 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov3; cov3 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_x3 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *x3, delta3, cov3); + auto relative_x2_x3 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *x3, delta3, cov3); fuse_core::Vector4d delta4; delta4 << 0.979795897, 0.2, 0.0, 0.0; fuse_core::Matrix3d cov4; cov4 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_l1 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *l1, delta4, cov4); + auto relative_x2_l1 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *l1, delta4, cov4); // Add to the graph auto graph = fuse_graphs::HashGraph(); @@ -508,26 +519,24 @@ TEST(MarginalizeVariables, MarginalizeVariables) auto expected_x3 = x3->array(); auto expected_l1 = l1->array(); - auto requests = std::vector> - { - {x2->uuid(), x2->uuid()}, {x3->uuid(), x3->uuid()}, {l1->uuid(), l1->uuid()} - }; + auto requests = std::vector>{ { x2->uuid(), x2->uuid() }, + { x3->uuid(), x3->uuid() }, + { l1->uuid(), l1->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto & expected_x2_cov = expected_covariances[0]; - const auto & expected_x3_cov = expected_covariances[1]; - const auto & expected_l1_cov = expected_covariances[2]; + const auto& expected_x2_cov = expected_covariances[0]; + const auto& expected_x3_cov = expected_covariances[1]; + const auto& expected_l1_cov = expected_covariances[2]; // Marginalize out X1 - auto transaction = fuse_constraints::marginalizeVariables("test", {x1->uuid()}, graph); // NOLINT + auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid() }, graph); // NOLINT // Verify the computed transaction auto added_variables = transaction.addedVariables(); EXPECT_EQ(0u, std::distance(added_variables.begin(), added_variables.end())); auto removed_variables_range = transaction.removedVariables(); - auto removed_variables = std::set( - removed_variables_range.begin(), removed_variables_range.end()); + auto removed_variables = std::set(removed_variables_range.begin(), removed_variables_range.end()); EXPECT_EQ(1u, removed_variables.size()); EXPECT_TRUE(removed_variables.count(x1->uuid())); @@ -535,9 +544,8 @@ TEST(MarginalizeVariables, MarginalizeVariables) EXPECT_EQ(1u, std::distance(added_constraints.begin(), added_constraints.end())); auto removed_constraints_range = transaction.removedConstraints(); - auto removed_constraints = std::set( - removed_constraints_range.begin(), - removed_constraints_range.end()); + auto removed_constraints = + std::set(removed_constraints_range.begin(), removed_constraints_range.end()); EXPECT_EQ(2u, removed_constraints.size()); EXPECT_TRUE(removed_constraints.count(prior_x1->uuid())); EXPECT_TRUE(removed_constraints.count(relative_x1_x2->uuid())); @@ -555,35 +563,41 @@ TEST(MarginalizeVariables, MarginalizeVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto & actual_x2_cov = actual_covariances[0]; - const auto & actual_x3_cov = actual_covariances[1]; - const auto & actual_l1_cov = actual_covariances[2]; + const auto& actual_x2_cov = actual_covariances[0]; + const auto& actual_x3_cov = actual_covariances[1]; + const auto& actual_l1_cov = actual_covariances[2]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); - for (size_t i = 0; i < expected_x2.size(); ++i) { + for (size_t i = 0; i < expected_x2.size(); ++i) + { EXPECT_NEAR(expected_x2[i], actual_x2[i], 1.0e-5); } ASSERT_EQ(expected_x2_cov.size(), actual_x2_cov.size()); - for (size_t i = 0; i < expected_x2_cov.size(); ++i) { + for (size_t i = 0; i < expected_x2_cov.size(); ++i) + { EXPECT_NEAR(expected_x2_cov[i], actual_x2_cov[i], 1.0e-5); } ASSERT_EQ(expected_x3.size(), actual_x3.size()); - for (size_t i = 0; i < expected_x3.size(); ++i) { + for (size_t i = 0; i < expected_x3.size(); ++i) + { EXPECT_NEAR(expected_x3[i], actual_x3[i], 1.0e-5); } ASSERT_EQ(expected_x3_cov.size(), actual_x3_cov.size()); - for (size_t i = 0; i < expected_x3_cov.size(); ++i) { + for (size_t i = 0; i < expected_x3_cov.size(); ++i) + { EXPECT_NEAR(expected_x3_cov[i], actual_x3_cov[i], 1.0e-5); } ASSERT_EQ(expected_l1.size(), actual_l1.size()); - for (size_t i = 0; i < expected_l1.size(); ++i) { + for (size_t i = 0; i < expected_l1.size(); ++i) + { EXPECT_NEAR(expected_l1[i], actual_l1[i], 1.0e-5); } ASSERT_EQ(expected_l1_cov.size(), actual_l1_cov.size()); - for (size_t i = 0; i < expected_l1_cov.size(); ++i) { + for (size_t i = 0; i < expected_l1_cov.size(); ++i) + { EXPECT_NEAR(expected_l1_cov[i], actual_l1_cov[i], 1.0e-5); } } @@ -617,8 +631,7 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) mean1 << 0.92736185, 0.1, 0.2, 0.3; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *x1, mean1, cov1); + auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *x1, mean1, cov1); // Note that this prior on the landmark is required. The covariance of the prior has no impact on // the solution, as the value of the landmark will be held constant. However, due to assumptions @@ -628,29 +641,28 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) mean2 << 0.842614977, 0.2, 0.3, 0.4; fuse_core::Matrix3d cov2; cov2 << 3.0, 0.0, 0.0, 0.0, 3.1, 0.0, 0.0, 0.0, 3.2; - auto prior_l1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *l1, mean2, cov2); + auto prior_l1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *l1, mean2, cov2); fuse_core::Vector4d delta3; delta3 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov3; cov3 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_x2 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *x2, delta3, cov3); + auto relative_x1_x2 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta3, cov3); fuse_core::Vector4d delta4; delta4 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov4; cov4 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_x3 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *x3, delta4, cov4); + auto relative_x2_x3 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *x3, delta4, cov4); fuse_core::Vector4d delta5; delta5 << 0.979795897, 0.2, 0.0, 0.0; fuse_core::Matrix3d cov5; cov5 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_l1 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *l1, delta5, cov5); + auto relative_x1_l1 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *l1, delta5, cov5); // Add to the graph auto graph = fuse_graphs::HashGraph(); @@ -671,26 +683,22 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) auto expected_x2 = x2->array(); auto expected_x3 = x3->array(); - auto requests = std::vector> - { - {x2->uuid(), x2->uuid()}, {x3->uuid(), x3->uuid()} - }; + auto requests = std::vector>{ { x2->uuid(), x2->uuid() }, + { x3->uuid(), x3->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto & expected_x2_cov = expected_covariances[0]; - const auto & expected_x3_cov = expected_covariances[1]; + const auto& expected_x2_cov = expected_covariances[0]; + const auto& expected_x3_cov = expected_covariances[1]; // Marginalize out X1 and L1 - auto transaction = - fuse_constraints::marginalizeVariables("test", {x1->uuid(), l1->uuid()}, graph); // NOLINT + auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid(), l1->uuid() }, graph); // NOLINT // Verify the computed transaction auto added_variables = transaction.addedVariables(); EXPECT_EQ(0u, std::distance(added_variables.begin(), added_variables.end())); auto removed_variables_range = transaction.removedVariables(); - auto removed_variables = std::set( - removed_variables_range.begin(), removed_variables_range.end()); + auto removed_variables = std::set(removed_variables_range.begin(), removed_variables_range.end()); EXPECT_EQ(2u, removed_variables.size()); EXPECT_TRUE(removed_variables.count(x1->uuid())); EXPECT_TRUE(removed_variables.count(l1->uuid())); @@ -699,9 +707,8 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) EXPECT_EQ(1u, std::distance(added_constraints.begin(), added_constraints.end())); auto removed_constraints_range = transaction.removedConstraints(); - auto removed_constraints = std::set( - removed_constraints_range.begin(), - removed_constraints_range.end()); + auto removed_constraints = + std::set(removed_constraints_range.begin(), removed_constraints_range.end()); EXPECT_EQ(4u, removed_constraints.size()); EXPECT_TRUE(removed_constraints.count(prior_x1->uuid())); @@ -721,25 +728,29 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto & actual_x2_cov = actual_covariances[0]; - const auto & actual_x3_cov = actual_covariances[1]; + const auto& actual_x2_cov = actual_covariances[0]; + const auto& actual_x3_cov = actual_covariances[1]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); - for (size_t i = 0; i < expected_x2.size(); ++i) { + for (size_t i = 0; i < expected_x2.size(); ++i) + { EXPECT_NEAR(expected_x2[i], actual_x2[i], 1.0e-3); } ASSERT_EQ(expected_x2_cov.size(), actual_x2_cov.size()); - for (size_t i = 0; i < expected_x2_cov.size(); ++i) { + for (size_t i = 0; i < expected_x2_cov.size(); ++i) + { EXPECT_NEAR(expected_x2_cov[i], actual_x2_cov[i], 1.0e-3); } ASSERT_EQ(expected_x3.size(), actual_x3.size()); - for (size_t i = 0; i < expected_x3.size(); ++i) { + for (size_t i = 0; i < expected_x3.size(); ++i) + { EXPECT_NEAR(expected_x3[i], actual_x3[i], 1.0e-3); } ASSERT_EQ(expected_x3_cov.size(), actual_x3_cov.size()); - for (size_t i = 0; i < expected_x3_cov.size(); ++i) { + for (size_t i = 0; i < expected_x3_cov.size(); ++i) + { EXPECT_NEAR(expected_x3_cov[i], actual_x3_cov[i], 1.0e-3); } } diff --git a/fuse_constraints/test/test_normal_delta_pose_2d.cpp b/fuse_constraints/test/test_normal_delta_pose_2d.cpp index 71ba0641d..45f0f5732 100644 --- a/fuse_constraints/test/test_normal_delta_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_delta_pose_2d.cpp @@ -51,8 +51,7 @@ class NormalDeltaPose2DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 2d cost functor using AutoDiffNormalDeltaPose2D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,25 +62,24 @@ class NormalDeltaPose2DTestFixture : public ::testing::Test } const fuse_core::Matrix3d covariance = - fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, - //!< y and yaw components - Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y - //!< and yaw components - const Eigen::Vector3d full_delta{1.0, 2.0, 3.0}; //!< The full pose 2d delta components: x, y and - //!< yaw + fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, + //!< y and yaw components + Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y + //!< and yaw components + const Eigen::Vector3d full_delta{ 1.0, 2.0, 3.0 }; //!< The full pose 2d delta components: x, y and + //!< yaw }; TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function - const fuse_constraints::NormalDeltaPose2D cost_function{full_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ full_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(full_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(full_sqrt_information, full_delta), num_residuals); // Compare the expected, automatic differentiation, cost function and the actual one ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); @@ -92,9 +90,9 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // Create cost function for each possible pair of two residuals, the ones in each possible pair of // rows using IndicesPair = std::array; - std::array indices_pairs = {IndicesPair{0, 1}, IndicesPair{0, 2}, IndicesPair{1, 2}}; - for (const auto & indices_pair : indices_pairs) { + std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; + for (const auto& indices_pair : indices_pairs) + { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // // https://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html @@ -104,18 +102,18 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // const fuse_core::Matrix partial_sqrt_information = // full_sqrt_information(indices_pair, Eigen::all); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices_pair.size(); ++i) { + for (size_t i = 0; i < indices_pair.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices_pair[i]); } - const fuse_constraints::NormalDeltaPose2D cost_function{partial_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } @@ -124,19 +122,19 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForOneResidual) { // Create cost function for one residual, the one in each row - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { SCOPED_TRACE("Residual " + std::to_string(i)); const fuse_core::Matrix partial_sqrt_information = full_sqrt_information.row(i); - const fuse_constraints::NormalDeltaPose2D cost_function{partial_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } diff --git a/fuse_constraints/test/test_normal_prior_pose_2d.cpp b/fuse_constraints/test/test_normal_prior_pose_2d.cpp index 151e3172d..605c228b5 100644 --- a/fuse_constraints/test/test_normal_prior_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_2d.cpp @@ -51,8 +51,7 @@ class NormalPriorPose2DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 2d cost functor using AutoDiffNormalPriorPose2D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,25 +62,24 @@ class NormalPriorPose2DTestFixture : public ::testing::Test } const fuse_core::Matrix3d covariance = - fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, - //!< y and yaw components - Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y - //!< and yaw components - const Eigen::Vector3d full_mean{1.0, 2.0, 3.0}; //!< The full pose 2d mean components: x, y and - //!< yaw + fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, + //!< y and yaw components + Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y + //!< and yaw components + const Eigen::Vector3d full_mean{ 1.0, 2.0, 3.0 }; //!< The full pose 2d mean components: x, y and + //!< yaw }; TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function - const fuse_constraints::NormalPriorPose2D cost_function{full_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ full_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(full_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(full_sqrt_information, full_mean), num_residuals); // Compare the expected, automatic differentiation, cost function and the actual one ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); @@ -92,9 +90,9 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // Create cost function for each possible pair of two residuals, the ones in each possible pair of // rows using IndicesPair = std::array; - std::array indices_pairs = {IndicesPair{0, 1}, IndicesPair{0, 2}, IndicesPair{1, 2}}; - for (const auto & indices_pair : indices_pairs) { + std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; + for (const auto& indices_pair : indices_pairs) + { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // // https://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html @@ -104,18 +102,18 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // const fuse_core::Matrix partial_sqrt_information = // full_sqrt_information(indices_pair, Eigen::all); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices_pair.size(); ++i) { + for (size_t i = 0; i < indices_pair.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices_pair[i]); } - const fuse_constraints::NormalPriorPose2D cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } @@ -124,19 +122,19 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForOneResidual) { // Create cost function for one residual, the one in each row - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { SCOPED_TRACE("Residual " + std::to_string(i)); const fuse_core::Matrix partial_sqrt_information = full_sqrt_information.row(i); - const fuse_constraints::NormalPriorPose2D cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 721fba58a..40b08c4ac 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -58,98 +58,69 @@ TEST(RelativeConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. { - fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); - fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint constraint( - "test", x1, x2, - delta, cov)); + fuse_constraints::RelativeAccelerationAngular2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bender")); - fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("bender")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; EXPECT_NO_THROW( - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint( - "test", x1, x2, - delta, cov)); + fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::Orientation2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("johnny5")); - fuse_variables::Orientation2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("johnny5")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); 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(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "rosie")); - fuse_variables::Position2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate( - "rosie")); + fuse_variables::Position2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("rosie")); + fuse_variables::Position2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("rosie")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); 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(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "clank")); - fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate( - "clank")); + fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("clank")); + fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("clank")); fuse_core::VectorXd delta(3); delta << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov(3, 3); 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(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("gort")); - fuse_variables::VelocityAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("gort")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - fuse_constraints::RelativeVelocityAngular2DStampedConstraint constraint( - "test", x1, x2, delta, - cov)); + EXPECT_NO_THROW(fuse_constraints::RelativeVelocityAngular2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::VelocityLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bishop")); - fuse_variables::VelocityLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("bishop")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); 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)); } } @@ -161,11 +132,8 @@ TEST(RelativeConstraint, PartialMeasurement) delta << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - EXPECT_NO_THROW( - fuse_constraints::RelativePosition3DStampedConstraint constraint( - "test", x1, x2, delta, cov, - indices)); + auto indices = std::vector{ 2, 0 }; + EXPECT_NO_THROW(fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, indices)); } TEST(RelativeConstraint, Covariance) @@ -173,20 +141,16 @@ TEST(RelativeConstraint, Covariance) // Test the covariance of a full measurement { // Verify the covariance <--> sqrt information conversions are correct - fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("chappie")); - fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("chappie")); + fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("chappie")); + fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("chappie")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, - delta, cov); + 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)); @@ -194,17 +158,14 @@ TEST(RelativeConstraint, Covariance) } // Test the covariance of a partial measurement { - fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("astroboy")); - fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("astroboy")); fuse_core::VectorXd delta(2); delta << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, - indices); + 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; expected_delta << 1.0, 0.0, 3.0; @@ -212,8 +173,8 @@ TEST(RelativeConstraint, Covariance) 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); /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ // Compare EXPECT_TRUE(expected_delta.isApprox(constraint.delta(), 1.0e-9)); @@ -229,14 +190,12 @@ 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::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto x1 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("t800")); x1->x() = 10.7; x1->y() = -3.2; - auto x2 = fuse_variables::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t800")); + auto x2 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1235, 5678), + fuse_core::uuid::generate("t800")); x2->x() = -4.2; x2->y() = 1.9; // Create an absolute constraint @@ -244,55 +203,37 @@ TEST(RelativeConstraint, Optimization) mean << 1.0, 2.0; fuse_core::MatrixXd cov1(2, 2); 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::VectorXd delta(2); delta << 0.1, 0.2; fuse_core::MatrixXd cov2(2, 2); 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_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; @@ -303,7 +244,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; @@ -331,15 +272,13 @@ 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( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t1000")); + auto x1 = + fuse_variables::Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t1000")); + auto x2 = + fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("t1000")); x2->x() = -4.2; x2->y() = 1.9; x2->z() = 19.2; @@ -348,75 +287,48 @@ TEST(RelativeConstraint, Optimization) mean1 << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov1(3, 3); 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::VectorXd delta2(3); delta2 << 0.1, 0.2, 0.3; fuse_core::MatrixXd cov2(3, 3); 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::VectorXd delta3(2); delta3 << 0.1, 0.2; fuse_core::MatrixXd cov3(2, 2); 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector c1_parameter_blocks; + std::vector c1_parameter_blocks; c1_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - c1->costFunction(), - c1->lossFunction(), - c1_parameter_blocks); - std::vector c2_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); - std::vector c3_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; @@ -429,7 +341,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; @@ -457,68 +369,47 @@ 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( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto x1 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("t800")); x1->yaw() = 0.7; - auto x2 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t800")); + auto x2 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("t800")); x2->yaw() = -2.2; // Create an absolute constraint fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov1(1, 1); 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::VectorXd delta(1); delta << 0.1; fuse_core::MatrixXd cov2(1, 1); 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; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_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; @@ -527,7 +418,7 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) EXPECT_NEAR(1.0, x1->yaw(), 1.0e-5); EXPECT_NEAR(1.1, x2->yaw(), 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; @@ -552,16 +443,13 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) TEST(RelativeConstraint, Serialization) { // Construct a constraint - fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); - fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint expected("test", x1, x2, delta, - cov); + fuse_constraints::RelativeAccelerationAngular2DStampedConstraint expected("test", x1, x2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; 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 014ad5387..91b2a85d2 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -49,11 +49,10 @@ #include #include -using fuse_variables::Orientation2DStamped; -using fuse_variables::Position2DStamped; using fuse_constraints::AbsolutePose2DStampedConstraint; using fuse_constraints::RelativePose2DStampedConstraint; - +using fuse_variables::Orientation2DStamped; +using fuse_variables::Position2DStamped; TEST(RelativePose2DStampedConstraint, Constructor) { @@ -67,9 +66,7 @@ TEST(RelativePose2DStampedConstraint, Constructor) 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( - RelativePose2DStampedConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + RelativePose2DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov)); } TEST(RelativePose2DStampedConstraint, Covariance) @@ -83,20 +80,12 @@ 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; /* *INDENT-OFF* */ - 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; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; // Compare @@ -109,18 +98,14 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto orientation1 = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation1 = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); orientation1->yaw() = 0.8; - auto position1 = - Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; - auto orientation2 = Orientation2DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation2 = Orientation2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); orientation2->yaw() = -2.7; - auto position2 = - Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; // Create an absolute pose constraint at the origin @@ -128,77 +113,52 @@ 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; fuse_core::Matrix3d cov2; cov2 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto relative = RelativePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - delta2, - cov2); + auto relative = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta2, cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + 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; @@ -212,7 +172,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(0.0, orientation2->yaw(), 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()); @@ -222,13 +182,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) std::vector covariance_vector1(position1->size() * position1->size()); covariance.GetCovarianceBlock(position1->data(), position1->data(), covariance_vector1.data()); std::vector covariance_vector2(position1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - position1->data(), orientation1->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position1->data(), orientation1->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - orientation1->data(), - orientation1->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation1->data(), orientation1->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -245,7 +201,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()); @@ -255,13 +211,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) std::vector covariance_vector1(position2->size() * position2->size()); covariance.GetCovarianceBlock(position2->data(), position2->data(), covariance_vector1.data()); std::vector covariance_vector2(position2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - position2->data(), orientation2->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position2->data(), orientation2->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - orientation2->data(), - orientation2->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation2->data(), orientation2->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -284,19 +236,15 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto orientation1 = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation1 = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); orientation1->yaw() = 0.8; - auto position1 = - Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; - auto orientation2 = Orientation2DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation2 = Orientation2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); orientation2->yaw() = -2.7; - auto position2 = - Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; @@ -305,107 +253,69 @@ 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}; - auto relative1 = RelativePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - delta1, - cov_rel1, - axes_lin1, - axes_ang1); + std::vector axes_lin1 = { fuse_variables::Position2DStamped::X }; + std::vector axes_ang1 = { fuse_variables::Orientation2DStamped::YAW }; + auto relative1 = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta1, cov_rel1, axes_lin1, axes_ang1); // Create a relative pose constraint for 0m in the y direction fuse_core::Vector1d delta2; 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", - *position1, - *orientation1, - *position2, - *orientation2, - delta2, - cov_rel2, - axes_lin2, - axes_ang2); + auto relative2 = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta2, cov_rel2, axes_lin2, axes_ang2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + 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; + 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; @@ -422,7 +332,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()); @@ -434,13 +344,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) std::vector covariance_vector1(position1->size() * position1->size()); covariance.GetCovarianceBlock(position1->data(), position1->data(), covariance_vector1.data()); std::vector covariance_vector2(position1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - position1->data(), orientation1->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position1->data(), orientation1->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - orientation1->data(), - orientation1->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation1->data(), orientation1->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -458,7 +364,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()); @@ -470,13 +376,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) std::vector covariance_vector1(position2->size() * position2->size()); covariance.GetCovarianceBlock(position2->data(), position2->data(), covariance_vector1.data()); std::vector covariance_vector2(position2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - position2->data(), orientation2->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position2->data(), orientation2->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - orientation2->data(), - orientation2->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation2->data(), orientation2->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -506,8 +408,7 @@ TEST(RelativePose2DStampedConstraint, Serialization) 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 expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose2DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; 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 7e16d7e56..0ddee00cc 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -49,11 +49,10 @@ #include #include -using fuse_variables::Orientation3DStamped; -using fuse_variables::Position3DStamped; using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_constraints::RelativePose3DStampedConstraint; - +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; TEST(RelativePose3DStampedConstraint, Constructor) { @@ -70,18 +69,22 @@ TEST(RelativePose3DStampedConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ EXPECT_NO_THROW( - RelativePose3DStampedConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov)); } TEST(RelativePose3DStampedConstraint, Covariance) @@ -98,32 +101,32 @@ TEST(RelativePose3DStampedConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov); + 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; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -137,27 +140,23 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto position1 = - Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position1->x() = 1.5; position1->y() = -3.0; position1->z() = 10.0; - auto orientation1 = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation1->w() = 0.952; orientation1->x() = 0.038; orientation1->y() = -0.189; orientation1->z() = 0.239; - auto position2 = - Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); position2->x() = -1.5; position2->y() = 3.0; position2->z() = -10.0; - auto orientation2 = Orientation3DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; @@ -167,78 +166,53 @@ 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; mean_delta << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); - auto relative = RelativePose3DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta); + auto relative = RelativePose3DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + 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; @@ -263,7 +237,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()); @@ -276,14 +250,10 @@ TEST(RelativePose3DStampedConstraint, Optimization) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -292,13 +262,8 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + expected_covariance << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -306,7 +271,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // 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()); @@ -319,14 +284,10 @@ TEST(RelativePose3DStampedConstraint, Optimization) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -335,13 +296,8 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + expected_covariance << 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -363,16 +319,21 @@ TEST(RelativePose3DStampedConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_uuid_ordering.cpp b/fuse_constraints/test/test_uuid_ordering.cpp index 508731a64..22c103340 100644 --- a/fuse_constraints/test/test_uuid_ordering.cpp +++ b/fuse_constraints/test/test_uuid_ordering.cpp @@ -46,11 +46,11 @@ TEST(UuidOrdering, Constructor) EXPECT_NO_THROW(UuidOrdering()); // Iterators - std::vector uuids{fuse_core::uuid::generate(), fuse_core::uuid::generate()}; + std::vector uuids{ fuse_core::uuid::generate(), fuse_core::uuid::generate() }; EXPECT_NO_THROW(UuidOrdering(uuids.begin(), uuids.end())); // Initializer List - EXPECT_NO_THROW(UuidOrdering({fuse_core::uuid::generate(), fuse_core::uuid::generate()})); // NOLINT + EXPECT_NO_THROW(UuidOrdering({ fuse_core::uuid::generate(), fuse_core::uuid::generate() })); // NOLINT } TEST(UuidOrdering, Access) @@ -59,7 +59,7 @@ TEST(UuidOrdering, Access) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_EQ(0u, order.at(uuid1)); EXPECT_EQ(1u, order.at(uuid2)); @@ -87,7 +87,7 @@ TEST(UuidOrdering, PushBack) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_EQ(3u, order.size()); EXPECT_FALSE(order.push_back(uuid3)); @@ -118,7 +118,7 @@ TEST(UuidOrdering, Exists) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_TRUE(order.exists(0)); EXPECT_TRUE(order.exists(1)); diff --git a/fuse_constraints/test/test_variable_constraints.cpp b/fuse_constraints/test/test_variable_constraints.cpp index 2edb0144b..b640854a6 100644 --- a/fuse_constraints/test/test_variable_constraints.cpp +++ b/fuse_constraints/test/test_variable_constraints.cpp @@ -48,9 +48,9 @@ TEST(VariableConstraints, Size) EXPECT_TRUE(vars.empty()); EXPECT_EQ(0u, vars.size()); - vars.insert(0u, {0u, 1u}); // NOLINT - vars.insert(1u, {0u, 1u}); // NOLINT - vars.insert(2u, {0u, 1u}); // NOLINT + vars.insert(0u, { 0u, 1u }); // NOLINT + vars.insert(1u, { 0u, 1u }); // NOLINT + vars.insert(2u, { 0u, 1u }); // NOLINT EXPECT_FALSE(vars.empty()); EXPECT_EQ(6u, vars.size()); @@ -61,7 +61,7 @@ TEST(VariableConstraints, NextVariableIndex) auto vars = VariableConstraints(); EXPECT_EQ(0u, vars.nextVariableIndex()); - vars.insert(0u, {9u, 10u}); // NOLINT + vars.insert(0u, { 9u, 10u }); // NOLINT EXPECT_EQ(11u, vars.nextVariableIndex()); } @@ -70,15 +70,15 @@ TEST(VariableConstraints, GetConstraints) { auto vars = VariableConstraints(); - vars.insert(0u, {0u, 1u, 2u}); // NOLINT - vars.insert(1u, {0u, 2u}); // NOLINT - vars.insert(2u, {1u, 2u}); // NOLINT - vars.insert(3u, {2u, 3u}); // NOLINT + vars.insert(0u, { 0u, 1u, 2u }); // NOLINT + vars.insert(1u, { 0u, 2u }); // NOLINT + vars.insert(2u, { 1u, 2u }); // NOLINT + vars.insert(3u, { 2u, 3u }); // NOLINT - auto expected0 = std::vector{0u, 1u}; // NOLINT - auto expected1 = std::vector{0u, 2u}; // NOLINT - auto expected2 = std::vector{0u, 1u, 2u, 3u}; // NOLINT - auto expected3 = std::vector{3u}; // NOLINT + auto expected0 = std::vector{ 0u, 1u }; // NOLINT + auto expected1 = std::vector{ 0u, 2u }; // NOLINT + auto expected2 = std::vector{ 0u, 1u, 2u, 3u }; // NOLINT + auto expected3 = std::vector{ 3u }; // NOLINT std::vector actual0; vars.getConstraints(0u, std::back_inserter(actual0)); @@ -113,18 +113,10 @@ TEST(VariableConstraints, GetConstraints) auto actual3_iter = actual3.begin(); actual3_iter = vars.getConstraints(3u, actual3_iter); - EXPECT_EQ( - static_cast(expected0.size()), - std::distance(actual0.begin(), actual0_iter)); - EXPECT_EQ( - static_cast(expected1.size()), - std::distance(actual1.begin(), actual1_iter)); - EXPECT_EQ( - static_cast(expected2.size()), - std::distance(actual2.begin(), actual2_iter)); - EXPECT_EQ( - static_cast(expected3.size()), - std::distance(actual3.begin(), actual3_iter)); + EXPECT_EQ(static_cast(expected0.size()), std::distance(actual0.begin(), actual0_iter)); + EXPECT_EQ(static_cast(expected1.size()), std::distance(actual1.begin(), actual1_iter)); + EXPECT_EQ(static_cast(expected2.size()), std::distance(actual2.begin(), actual2_iter)); + EXPECT_EQ(static_cast(expected3.size()), std::distance(actual3.begin(), actual3_iter)); } TEST(VariableConstraints, InsertOrphanVariable) diff --git a/fuse_core/include/fuse_core/async_motion_model.hpp b/fuse_core/include/fuse_core/async_motion_model.hpp index 1c836c3c0..cdbb091a4 100644 --- a/fuse_core/include/fuse_core/async_motion_model.hpp +++ b/fuse_core/include/fuse_core/async_motion_model.hpp @@ -112,7 +112,7 @@ class AsyncMotionModel : public MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - bool apply(Transaction & transaction) override; + bool apply(Transaction& transaction) override; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -142,14 +142,16 @@ class AsyncMotionModel : public MotionModel * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Get the unique name of this motion model */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -201,8 +203,8 @@ class AsyncMotionModel : public MotionModel //! A single/multi-threaded executor assigned to the local callback queue rclcpp::Executor::SharedPtr executor_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -232,7 +234,7 @@ class AsyncMotionModel : public MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - virtual bool applyCallback(Transaction & transaction) = 0; + virtual bool applyCallback(Transaction& transaction) = 0; /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -251,7 +253,9 @@ class AsyncMotionModel : public MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) {} + virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required initialization for the motion model @@ -260,7 +264,9 @@ class AsyncMotionModel : public MotionModel * The class's node will be properly initialized before onInit() is called. Spinning * of the callback queue will not begin until after the call to onInit() completes. */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Perform any required operations to prepare for servicing calls to apply() @@ -268,7 +274,9 @@ class AsyncMotionModel : public MotionModel * This function will be called once after initialize() but before any calls to apply(). It * may also be called at any time after a call to stop(). */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to clean up the internal state @@ -277,7 +285,9 @@ class AsyncMotionModel : public MotionModel * after a call to start(). No calls to apply() will occur after stop() is called, but before * start() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index 4df5fef6d..af016b583 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -86,14 +86,16 @@ class AsyncPublisher : public Publisher * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Get the unique name of this publisher */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Notify the publisher that an optimization cycle is complete, and about changes to the @@ -154,7 +156,7 @@ class AsyncPublisher : public Publisher //! The callback queue used for fuse internal callbacks std::shared_ptr callback_queue_; - std::string name_; //!< The unique name for this publisher instance + std::string name_; //!< The unique name for this publisher instance rclcpp::CallbackGroup::SharedPtr cb_group_; //!< Internal re-entrant callback group //! The node interfaces @@ -162,8 +164,8 @@ class AsyncPublisher : public Publisher //! A single/multi-threaded executor assigned to the local callback queue rclcpp::Executor::SharedPtr executor_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -185,7 +187,9 @@ class AsyncPublisher : public Publisher * Derived classes should override this method to implement any additional initialization * steps needed (access the parameter server, advertise, subscribe, etc.). */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Callback method executed in response to the optimizer completing an optimization cycle. @@ -199,9 +203,9 @@ class AsyncPublisher : public Publisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - virtual void notifyCallback( - Transaction::ConstSharedPtr /*transaction*/, - Graph::ConstSharedPtr /*graph*/) {} + virtual void notifyCallback(Transaction::ConstSharedPtr /*transaction*/, Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required operations to prepare for servicing calls to notify() @@ -209,7 +213,9 @@ class AsyncPublisher : public Publisher * This function will be called once after initialize() but before any calls to notify(). It * may also be called at any time after a call to stop(). */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to clean up the internal state @@ -218,7 +224,9 @@ class AsyncPublisher : public Publisher * after a call to start(). No calls to notify() will occur after stop() is called, but * before start() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index c82748fab..c550fe72e 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -127,10 +127,8 @@ class AsyncSensorModel : public SensorModel * @param[in] transaction_callback The function to call every time a transaction is published * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) override; + void initialize(node_interfaces::NodeInterfaces interfaces, const std::string& name, + TransactionCallback transaction_callback) override; /** * @brief Send a transaction to the Optimizer @@ -152,7 +150,10 @@ class AsyncSensorModel : public SensorModel /** * @brief Get the unique name of this sensor */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -205,8 +206,8 @@ class AsyncSensorModel : public SensorModel //! The function to be executed every time a Transaction is "published" TransactionCallback transaction_callback_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -235,7 +236,9 @@ class AsyncSensorModel : public SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) {} + virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required initialization for the sensor model @@ -247,7 +250,9 @@ class AsyncSensorModel : public SensorModel * Derived sensor models classes must implement this function, because otherwise I'm not sure * how the derived sensor model would actually do anything. */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Perform any required operations to prepare for sending transactions to the optimizer @@ -258,7 +263,9 @@ class AsyncSensorModel : public SensorModel * The sensor model must not send any transactions to the optimizer before onStart() is * called. */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to stop sending transactions to the optimizer @@ -268,7 +275,9 @@ class AsyncSensorModel : public SensorModel * * The sensor model must not send any transactions to the optimizer after stop() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.h b/fuse_core/include/fuse_core/autodiff_local_parameterization.h index 104404a1b..f3135b8c2 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.h +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.h @@ -35,8 +35,7 @@ #ifndef FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ #define FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ -#warning \ - This header is obsolete, please include fuse_core/autodiff_local_parameterization.hpp instead +#warning This header is obsolete, please include fuse_core/autodiff_local_parameterization.hpp instead #include diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp index 2fa781cb9..e532c8097 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp @@ -73,13 +73,11 @@ namespace fuse_core * * For more information on local parameterizations, see fuse_core::LocalParameterization */ -template +template class AutoDiffLocalParameterization : public LocalParameterization { public: - FUSE_SMART_PTR_DEFINITIONS( - AutoDiffLocalParameterization) + FUSE_SMART_PTR_DEFINITIONS(AutoDiffLocalParameterization) /** * @brief Constructs new PlusFunctor and MinusFunctor instances @@ -89,7 +87,7 @@ class AutoDiffLocalParameterization : public LocalParameterization /** * @brief Takes ownership of the provided PlusFunctor and MinusFunctor instances */ - AutoDiffLocalParameterization(PlusFunctor * plus_functor, MinusFunctor * minus_functor); + AutoDiffLocalParameterization(PlusFunctor* plus_functor, MinusFunctor* minus_functor); /** * @brief Generalization of the addition operation, implemented by the provided PlusFunctor @@ -99,10 +97,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] x_plus_delta The final variable value, of size \p GlobalSize() * @return True if successful, false otherwise */ - 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; /** * @brief The Jacobian of Plus(x, delta) w.r.t delta at delta = 0, computed using automatic @@ -112,9 +107,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p GlobalSize() x \p LocalSize() * @return True is successful, false otherwise */ - bool ComputeJacobian( - const double * x, - double * jacobian) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; /** * @brief Generalization of the subtraction operation, implemented by the provided MinusFunctor @@ -125,10 +118,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * LocalSize() * @return True if successful, false otherwise */ - bool Minus( - const double * x1, - const double * x2, - double * delta) const override; + bool Minus(const double* x1, const double* x2, double* delta) const override; /** * @brief The Jacobian of Minus(x1, x2) w.r.t x2 evaluated at x1 = x2 = x, computed using @@ -137,99 +127,91 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p LocalSize() x \p GlobalSize() * @return True is successful, false otherwise */ - bool ComputeMinusJacobian( - const double * x, - double * jacobian) const override; + bool ComputeMinusJacobian(const double* x, double* jacobian) const override; /** * @brief The size of the variable parameterization in the nonlinear manifold */ - int GlobalSize() const override {return kGlobalSize;} + int GlobalSize() const override + { + return kGlobalSize; + } /** * @brief The size of a delta vector in the linear tangent space to the nonlinear manifold */ - int LocalSize() const override {return kLocalSize;} + int LocalSize() const override + { + return kLocalSize; + } private: std::unique_ptr plus_functor_; std::unique_ptr minus_functor_; }; -template -AutoDiffLocalParameterization::AutoDiffLocalParameterization() -: plus_functor_(new PlusFunctor()), - minus_functor_(new MinusFunctor()) +template +AutoDiffLocalParameterization::AutoDiffLocalParameterization() + : plus_functor_(new PlusFunctor()), minus_functor_(new MinusFunctor()) { } -template -AutoDiffLocalParameterization::AutoDiffLocalParameterization( - PlusFunctor * plus_functor, - MinusFunctor * minus_functor) -: plus_functor_(plus_functor), - minus_functor_(minus_functor) +template +AutoDiffLocalParameterization::AutoDiffLocalParameterization( + PlusFunctor* plus_functor, MinusFunctor* minus_functor) + : plus_functor_(plus_functor), minus_functor_(minus_functor) { } -template -bool AutoDiffLocalParameterization::Plus( - const double * x, - const double * delta, - double * x_plus_delta) const +template +bool AutoDiffLocalParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { return (*plus_functor_)(x, delta, x_plus_delta); } -template -bool AutoDiffLocalParameterization::ComputeJacobian( - const double * x, - double * jacobian) const +template +bool AutoDiffLocalParameterization::ComputeJacobian( + const double* x, double* jacobian) const { double zero_delta[kLocalSize] = {}; // zero-initialize double x_plus_delta[kGlobalSize]; - const double * parameter_ptrs[2] = {x, zero_delta}; - double * jacobian_ptrs[2] = {NULL, jacobian}; + const double* parameter_ptrs[2] = { x, zero_delta }; + double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) - return ceres::internal::AutoDiff - ::Differentiate(*plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); + return ceres::internal::AutoDiff::Differentiate( + *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( - *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); + return ceres::internal::AutoDifferentiate >( + *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #endif } -template -bool AutoDiffLocalParameterization::Minus( - const double * x1, - const double * x2, - double * delta) const +template +bool AutoDiffLocalParameterization::Minus(const double* x1, + const double* x2, + double* delta) const { return (*minus_functor_)(x1, x2, delta); } -template -bool AutoDiffLocalParameterization::ComputeMinusJacobian( - const double * x, - double * jacobian) const +template +bool AutoDiffLocalParameterization::ComputeMinusJacobian( + const double* x, double* jacobian) const { double delta[kLocalSize] = {}; // zero-initialize - const double * parameter_ptrs[2] = {x, x}; - double * jacobian_ptrs[2] = {NULL, jacobian}; + const double* parameter_ptrs[2] = { x, x }; + double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) - return ceres::internal::AutoDiff - ::Differentiate(*minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); + return ceres::internal::AutoDiff::Differentiate( + *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #else using StaticParameters = ceres::internal::StaticParameterDims; - return ceres::internal::AutoDifferentiate( - *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); + return ceres::internal::AutoDifferentiate(*minus_functor_, parameter_ptrs, kLocalSize, + delta, jacobian_ptrs); #endif } diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 0d6532823..3180a8401 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -105,19 +105,18 @@ class CallbackWrapperBase virtual void call() = 0; }; -template +template class CallbackWrapper : public CallbackWrapperBase { public: - using CallbackFunction = std::function; + using CallbackFunction = std::function; /** * @brief Constructor * * @param[in] callback The function to be called from the callback queue */ - explicit CallbackWrapper(CallbackFunction callback) - : callback_(callback) + explicit CallbackWrapper(CallbackFunction callback) : callback_(callback) { } @@ -139,19 +138,18 @@ class CallbackWrapper : public CallbackWrapperBase private: CallbackFunction callback_; //!< The function to execute within the - std::promise promise_; //!< Promise/Future used to return data after the callback is executed + std::promise promise_; //!< Promise/Future used to return data after the callback is executed }; // Specialization to handle 'void' return types // Specifically, promise_.set_value(callback_()) does not work if callback_() returns void. -template<> +template <> inline void CallbackWrapper::call() { callback_(); promise_.set_value(); } - class CallbackAdapter : public rclcpp::Waitable { public: @@ -162,12 +160,10 @@ class CallbackAdapter : public rclcpp::Waitable */ size_t get_number_of_ready_guard_conditions() override; - /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ - bool is_ready(rcl_wait_set_t const & wait_set) override; - + bool is_ready(rcl_wait_set_t const& wait_set) override; /** * @brief add_to_wait_set is called by rclcpp during NodeWaitables::add_waitable() and @@ -175,15 +171,15 @@ class CallbackAdapter : public rclcpp::Waitable waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ - void add_to_wait_set(rcl_wait_set_t & wait_set) override; + void add_to_wait_set(rcl_wait_set_t& wait_set) override; std::shared_ptr take_data() override; - void execute(std::shared_ptr const & data) override; + void execute(std::shared_ptr const& data) override; - void addCallback(const std::shared_ptr & callback); + void addCallback(const std::shared_ptr& callback); - void addCallback(std::shared_ptr && callback); + void addCallback(std::shared_ptr&& callback); void removeAllCallbacks(); @@ -195,7 +191,6 @@ class CallbackAdapter : public rclcpp::Waitable std::deque> callback_queue_; }; - } // namespace fuse_core #endif // FUSE_CORE__CALLBACK_WRAPPER_HPP_ diff --git a/fuse_core/include/fuse_core/ceres_macros.hpp b/fuse_core/include/fuse_core/ceres_macros.hpp index d5c648204..a742c3656 100644 --- a/fuse_core/include/fuse_core/ceres_macros.hpp +++ b/fuse_core/include/fuse_core/ceres_macros.hpp @@ -41,9 +41,9 @@ */ /* *INDENT-OFF* */ // Bypass uncrustify -#define CERES_VERSION_AT_LEAST(x, y, z) (CERES_VERSION_MAJOR > x || (CERES_VERSION_MAJOR >= x && \ - (CERES_VERSION_MINOR > y || (CERES_VERSION_MINOR >= y && \ - CERES_VERSION_REVISION >= z)))) +#define CERES_VERSION_AT_LEAST(x, y, z) \ + (CERES_VERSION_MAJOR > x || (CERES_VERSION_MAJOR >= x && \ + (CERES_VERSION_MINOR > y || (CERES_VERSION_MINOR >= y && CERES_VERSION_REVISION >= z)))) /* *INDENT-ON* */ #define CERES_SUPPORTS_MANIFOLDS CERES_VERSION_AT_LEAST(2, 1, 0) diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp index e364b2118..58022f7a3 100644 --- a/fuse_core/include/fuse_core/ceres_options.hpp +++ b/fuse_core/include/fuse_core/ceres_options.hpp @@ -53,10 +53,10 @@ * * For a given Ceres Solver Option , the function ToString calls ceres::ToString */ -#define CERES_OPTION_TO_STRING_DEFINITION(Option) \ - static inline const char * ToString(ceres::Option value) \ - { \ - return ceres::Option ## ToString(value); \ +#define CERES_OPTION_TO_STRING_DEFINITION(Option) \ + static inline const char* ToString(ceres::Option value) \ + { \ + return ceres::Option##ToString(value); \ } /** @@ -64,10 +64,10 @@ * * For a given Ceres Solver Option , the function FromString calls ceres::StringTo */ -#define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ - static inline bool FromString(std::string string_value, ceres::Option * value) \ - { \ - return ceres::StringTo ## Option(string_value, value); \ +#define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ + static inline bool FromString(std::string string_value, ceres::Option* value) \ + { \ + return ceres::StringTo##Option(string_value, value); \ } /** @@ -75,8 +75,8 @@ * * See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION. */ -#define CERES_OPTION_STRING_DEFINITIONS(Option) \ - CERES_OPTION_TO_STRING_DEFINITION(Option) \ +#define CERES_OPTION_STRING_DEFINITIONS(Option) \ + CERES_OPTION_TO_STRING_DEFINITION(Option) \ CERES_OPTION_FROM_STRING_DEFINITION(Option) #if !CERES_VERSION_AT_LEAST(2, 0, 0) @@ -88,25 +88,33 @@ namespace ceres { -#define CASESTR(x) case x: return #x -#define STRENUM(x) if (value == #x) {*type = x; return true;} +#define CASESTR(x) \ + case x: \ + return #x +#define STRENUM(x) \ + if (value == #x) \ + { \ + *type = x; \ + return true; \ + } -static void UpperCase(std::string * input) +static void UpperCase(std::string* input) { std::transform(input->begin(), input->end(), input->begin(), ::toupper); } -inline const char * LoggingTypeToString(LoggingType type) +inline const char* LoggingTypeToString(LoggingType type) { - switch (type) { - CASESTR(SILENT); - CASESTR(PER_MINIMIZER_ITERATION); + switch (type) + { + CASESTR(SILENT); + CASESTR(PER_MINIMIZER_ITERATION); default: return "UNKNOWN"; } } -inline bool StringToLoggingType(std::string value, LoggingType * type) +inline bool StringToLoggingType(std::string value, LoggingType* type) { UpperCase(&value); STRENUM(SILENT); @@ -114,17 +122,18 @@ inline bool StringToLoggingType(std::string value, LoggingType * type) return false; } -inline const char * DumpFormatTypeToString(DumpFormatType type) +inline const char* DumpFormatTypeToString(DumpFormatType type) { - switch (type) { - CASESTR(CONSOLE); - CASESTR(TEXTFILE); + switch (type) + { + CASESTR(CONSOLE); + CASESTR(TEXTFILE); default: return "UNKNOWN"; } } -inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) +inline bool StringToDumpFormatType(std::string value, DumpFormatType* type) { UpperCase(&value); STRENUM(CONSOLE); @@ -144,12 +153,12 @@ inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) namespace ceres { -inline bool StringToLoggingType(std::string value, LoggingType * type) +inline bool StringToLoggingType(std::string value, LoggingType* type) { return StringtoLoggingType(value, type); } -inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) +inline bool StringToDumpFormatType(std::string value, DumpFormatType* type) { return StringtoDumpFormatType(value, type); } @@ -188,30 +197,25 @@ CERES_OPTION_STRING_DEFINITIONS(VisibilityClusteringType) * @param[in] default_value - A default value to use if the provided parameter name does not exist * @return The loaded (or default) value */ -template +template T declareCeresParam( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - const T & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + node_interfaces::NodeInterfaces + interfaces, + const std::string& parameter_name, const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor()) { - const std::string default_string_value{ToString(default_value)}; + const std::string default_string_value{ ToString(default_value) }; std::string string_value; string_value = getParam(interfaces, parameter_name, default_string_value, parameter_descriptor); T value; - if (!FromString(string_value, &value)) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested " << parameter_name << " (" << string_value - << ") is not supported. Using the default value (" << default_string_value - << ") instead."); + if (!FromString(string_value, &value)) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested " << parameter_name << " (" << string_value + << ") is not supported. Using the default value (" << default_string_value + << ") instead."); value = default_value; } @@ -227,13 +231,9 @@ T declareCeresParam( * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ void loadCovarianceOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Covariance::Options & covariance_options, - const std::string & ns = std::string()); + node_interfaces::NodeInterfaces + interfaces, + ceres::Covariance::Options& covariance_options, const std::string& ns = std::string()); /** * @brief Populate a ceres::Problem::Options object with information from the parameter server @@ -243,12 +243,8 @@ void loadCovarianceOptionsFromROS( * @param[out] problem_options - The ceres::Problem::Options object to update * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ -void loadProblemOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Parameters - > interfaces, - ceres::Problem::Options & problem_options, - const std::string & ns = std::string()); +void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, + ceres::Problem::Options& problem_options, const std::string& ns = std::string()); /** * @brief Populate a ceres::Solver::Options object with information from the parameter server @@ -259,13 +255,9 @@ void loadProblemOptionsFromROS( * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ void loadSolverOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Solver::Options & solver_options, - const std::string & ns = std::string()); + node_interfaces::NodeInterfaces + interfaces, + ceres::Solver::Options& solver_options, const std::string& ns = std::string()); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/console.hpp b/fuse_core/include/fuse_core/console.hpp index c30e6737b..9adae01f5 100644 --- a/fuse_core/include/fuse_core/console.hpp +++ b/fuse_core/include/fuse_core/console.hpp @@ -42,11 +42,11 @@ namespace fuse_core // To replicate ROS 1 behavior, the throttle checking conditions were adapted from the logic here: // https://github.com/ros/rosconsole/blob/c9503279e932a04b3d2667cca3d28a8133cacc22/include/ros/console.h #if defined(_MSC_VER) - #define FUSE_LIKELY(x) (x) - #define FUSE_UNLIKELY(x) (x) +#define FUSE_LIKELY(x) (x) +#define FUSE_UNLIKELY(x) (x) #else - #define FUSE_LIKELY(x) __builtin_expect((x), 1) - #define FUSE_UNLIKELY(x) __builtin_expect((x), 0) +#define FUSE_LIKELY(x) __builtin_expect((x), 1) +#define FUSE_UNLIKELY(x) __builtin_expect((x), 0) #endif /** @@ -62,8 +62,7 @@ class DelayedThrottleFilter * * @param[in] The throttle period in seconds */ - explicit DelayedThrottleFilter(const double period) - : period_(std::chrono::duration>(period)) + explicit DelayedThrottleFilter(const double period) : period_(std::chrono::duration>(period)) { reset(); } @@ -82,15 +81,16 @@ class DelayedThrottleFilter */ bool isEnabled() { - const auto now = std::chrono::time_point_cast( - std::chrono::system_clock::now()); + const auto now = std::chrono::time_point_cast(std::chrono::system_clock::now()); - if (last_hit_.time_since_epoch().count() < 0.0) { + if (last_hit_.time_since_epoch().count() < 0.0) + { last_hit_ = now; return true; } - if (FUSE_UNLIKELY(last_hit_ + period_ <= now) || FUSE_UNLIKELY(now < last_hit_)) { + if (FUSE_UNLIKELY(last_hit_ + period_ <= now) || FUSE_UNLIKELY(now < last_hit_)) + { last_hit_ = now; return true; } @@ -103,8 +103,7 @@ class DelayedThrottleFilter */ void reset() { - last_hit_ = std::chrono::time_point_cast( - std::chrono::system_clock::from_time_t(-1)); + last_hit_ = std::chrono::time_point_cast(std::chrono::system_clock::from_time_t(-1)); } private: diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 1abd6d16d..2e18ee7f0 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -66,10 +66,10 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ - fuse_core::Constraint::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ + fuse_core::Constraint::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -85,22 +85,22 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -118,17 +118,17 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** @@ -145,10 +145,10 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_CONSTRAINT_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) /** @@ -165,13 +165,12 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -213,15 +212,15 @@ class Constraint * * @param[in] variable_uuid_list The list of involved variable UUIDs */ - Constraint(const std::string & source, std::initializer_list variable_uuid_list); + Constraint(const std::string& source, std::initializer_list variable_uuid_list); /** * @brief Constructor * * Accepts an arbitrary number of variable UUIDs stored in a container using iterators. */ - template - Constraint(const std::string & source, VariableUuidIterator first, VariableUuidIterator last); + template + Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last); /** * @brief Destructor @@ -241,19 +240,25 @@ class Constraint * * Each constraint will generate a unique, random UUID during construction. */ - const UUID & uuid() const {return uuid_;} + const UUID& uuid() const + { + return uuid_; + } /** * @brief Returns the name of the sensor or motion model that generated this constraint */ - const std::string & source() const {return source_;} + const std::string& source() const + { + return source_; + } /** * @brief Print a human-readable description of the constraint to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Create a new Ceres cost function and return a raw pointer to it. @@ -266,7 +271,7 @@ class Constraint * * @return A base pointer to an instance of a derived ceres::CostFunction. */ - virtual ceres::CostFunction * costFunction() const = 0; + virtual ceres::CostFunction* costFunction() const = 0; /** * @brief Read-only access to the loss. @@ -296,7 +301,7 @@ class Constraint * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const + ceres::LossFunction* lossFunction() const { return loss_ ? loss_->lossFunction() : nullptr; } @@ -316,7 +321,10 @@ class Constraint /** * @brief Read-only access to the ordered list of variable UUIDs involved in this constraint */ - const std::vector & variables() const {return variables_;} + const std::vector& variables() const + { + return variables_; + } /** * @brief Serialize this Constraint into the provided binary archive @@ -328,7 +336,7 @@ class Constraint * * @param[out] archive - The archive to serialize this constraint into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Constraint into the provided text archive @@ -340,7 +348,7 @@ class Constraint * * @param[out] archive - The archive to serialize this constraint into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Constraint @@ -352,7 +360,7 @@ class Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Constraint @@ -364,13 +372,13 @@ class Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: - std::string source_; //!< The name of the sensor or motion model that generated this constraint - UUID uuid_; //!< The unique ID associated with this constraint - std::vector variables_; //!< The ordered set of variables involved with this constraint - std::shared_ptr loss_{nullptr}; //!< The loss function + std::string source_; //!< The name of the sensor or motion model that generated this constraint + UUID uuid_; //!< The unique ID associated with this constraint + std::vector variables_; //!< The ordered set of variables involved with this constraint + std::shared_ptr loss_{ nullptr }; //!< The loss function // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -387,29 +395,24 @@ class Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & source_; - archive & uuid_; - archive & variables_; - archive & loss_; + archive& source_; + archive& uuid_; + archive& variables_; + archive& loss_; } }; /** * Stream operator implementation used for all derived Constraint classes. */ -std::ostream & operator<<(std::ostream & stream, const Constraint & constraint); - +std::ostream& operator<<(std::ostream& stream, const Constraint& constraint); -template -Constraint::Constraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) -: source_(source), - uuid_(uuid::generate()), - variables_(first, last) +template +Constraint::Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : source_(source), uuid_(uuid::generate()), variables_(first, last) { } diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 6d9ced923..23d794559 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -66,7 +66,7 @@ using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; -template +template using Matrix = Eigen::Matrix; /** @@ -80,8 +80,8 @@ using Matrix = Eigen::Matrix -std::string to_string(const Eigen::DenseBase & m, const int precision = 4) +template +std::string to_string(const Eigen::DenseBase& m, const int precision = 4) { static const Eigen::IOFormat pretty(precision, 0, ", ", "\n", "[", "]"); @@ -98,11 +98,10 @@ std::string to_string(const Eigen::DenseBase & m, const int precision = * property used to check for symmetry. * @return True if the matrix m is symmetric; False, otherwise. */ -template -bool isSymmetric( - const Eigen::DenseBase & m, - const typename Eigen::DenseBase::RealScalar precision = - Eigen::NumTraits::Scalar>::dummy_precision()) +template +bool isSymmetric(const Eigen::DenseBase& m, + const typename Eigen::DenseBase::RealScalar precision = + Eigen::NumTraits::Scalar>::dummy_precision()) { // We do not use `isApprox`: // @@ -112,7 +111,7 @@ bool isSymmetric( // // See: // https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c - const auto & derived = m.derived(); + const auto& derived = m.derived(); return (derived - derived.transpose()).cwiseAbs().maxCoeff() < precision; } @@ -122,8 +121,8 @@ bool isSymmetric( * @param[in] m - Square matrix to check PD-ness on. * @return True if the matrix m is PD; False, otherwise. */ -template -bool isPositiveDefinite(const Eigen::DenseBase & m) +template +bool isPositiveDefinite(const Eigen::DenseBase& m) { Eigen::SelfAdjointEigenSolver solver(m); return solver.eigenvalues().minCoeff() > 0.0; diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp index 5fe3d760c..c29a3a767 100644 --- a/fuse_core/include/fuse_core/eigen_gtest.hpp +++ b/fuse_core/include/fuse_core/eigen_gtest.hpp @@ -58,21 +58,22 @@ namespace testing * @param[in] v2 Actual matrix * @return AssertionSuccess or AssertionFailure */ -template -AssertionResult AssertMatrixEqualHelper( - const char * e1, - const char * e2, - const Eigen::MatrixBase & v1, - const Eigen::MatrixBase & v2) +template +AssertionResult AssertMatrixEqualHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, + const Eigen::MatrixBase& v2) { - if (v1 == v2) { + if (v1 == v2) + { return AssertionSuccess(); } Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n" - << e2 << " is:\n" << v2.format(clean) << "\n" - << "Difference is:\n" << (v1 - v2).format(clean) << "\n"; + return AssertionFailure() << e1 << " is:\n" + << v1.format(clean) << "\n" + << e2 << " is:\n" + << v2.format(clean) << "\n" + << "Difference is:\n" + << (v1 - v2).format(clean) << "\n"; } /** @@ -87,44 +88,33 @@ AssertionResult AssertMatrixEqualHelper( * @param[in] tol Tolerance * @return AssertionSuccess or AssertionFailure */ -template -AssertionResult AssertMatrixNearHelper( - const char * e1, - const char * e2, - const Eigen::MatrixBase & v1, - const Eigen::MatrixBase & v2, - double tol) +template +AssertionResult AssertMatrixNearHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, + const Eigen::MatrixBase& v2, double tol) { - if ((v1 - v2).cwiseAbs().maxCoeff() < tol) { + if ((v1 - v2).cwiseAbs().maxCoeff() < tol) + { return AssertionSuccess(); } Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n" - << e2 << " is:\n" << v2.format(clean) << "\n" - << "Difference is:\n" << (v1 - v2).format(clean) << "\n"; + return AssertionFailure() << e1 << " is:\n" + << v1.format(clean) << "\n" + << e2 << " is:\n" + << v2.format(clean) << "\n" + << "Difference is:\n" + << (v1 - v2).format(clean) << "\n"; } // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ. // Don't use this in your code. -#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixEqualHelper( \ - #v1, \ - #v2, \ - v1, \ - v2), on_failure) +#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ + GTEST_ASSERT_(::testing::AssertMatrixEqualHelper(#v1, #v2, v1, v2), on_failure) // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR. // Don't use this in your code. -#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixNearHelper( \ - #v1, \ - #v2, \ - v1, \ - v2, \ - tol), on_failure) +#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ + GTEST_ASSERT_(::testing::AssertMatrixNearHelper(#v1, #v2, v1, v2, tol), on_failure) // Define gtest macros for use with Eigen @@ -136,8 +126,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v1 The expected matrix * @param[in] v2 The actual matrix */ -#define EXPECT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) +#define EXPECT_MATRIX_EQ(v1, v2) GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for exact equality of two Eigen matrix-like objects. @@ -147,8 +136,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v1 The expected matrix * @param[in] v2 The actual matrix */ -#define ASSERT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) +#define ASSERT_MATRIX_EQ(v1, v2) GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) /** * @brief Non-fatal check for approximate equality of two Eigen matrix-like objects. @@ -159,8 +147,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ -#define EXPECT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) +#define EXPECT_MATRIX_NEAR(v1, v2, tol) GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for approximate equality of two Eigen matrix-like objects. @@ -171,8 +158,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ -#define ASSERT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) +#define ASSERT_MATRIX_NEAR(v1, v2, tol) GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) } // namespace testing diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp index 707fdf101..6484d12f3 100644 --- a/fuse_core/include/fuse_core/fuse_macros.hpp +++ b/fuse_core/include/fuse_core/fuse_macros.hpp @@ -75,9 +75,9 @@ * definitions. */ #if __cpp_aligned_new - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() #else - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW #endif /** @@ -85,11 +85,11 @@ * * Use in the public section of the class. */ -#define FUSE_SMART_PTR_DEFINITIONS(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ +#define FUSE_SMART_PTR_DEFINITIONS(...) \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** @@ -103,16 +103,15 @@ * Use in the public section of the class. */ #if __cpp_aligned_new - #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) +#define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) #else - #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -123,56 +122,49 @@ * * Use in the public section of the class. */ -#define FUSE_SMART_PTR_ALIASES_ONLY(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ +#define FUSE_SMART_PTR_ALIASES_ONLY(...) \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) -#define __FUSE_SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ +#define __FUSE_SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ using ConstSharedPtr = std::shared_ptr; -#define __FUSE_MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ +#define __FUSE_MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args)...); \ } -#define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ +#define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>(Eigen::aligned_allocator<__VA_ARGS__>(), std::forward(args)...); \ } -#define __FUSE_WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ +#define __FUSE_WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ using ConstWeakPtr = std::weak_ptr; -#define __FUSE_UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; +#define __FUSE_UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L - #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } +#define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args)...); \ + } #else - #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } +#define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args)...)); \ + } #endif #endif // FUSE_CORE__FUSE_MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index 1317cd9b4..6e73ffc5d 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -70,22 +70,22 @@ * } * @endcode */ -#define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -103,38 +103,37 @@ * } * @endcode */ -#define FUSE_GRAPH_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_GRAPH_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** -* @brief Convenience function that creates the required pointer aliases, and type() method -* -* Usage: -* @code{.cpp} -* class Derived : public Graph -* { -* public: -* FUSE_GRAPH_DEFINITIONS(Derived) -* // The rest of the derived graph implementation -* } -* @endcode -*/ -#define FUSE_GRAPH_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ + * @brief Convenience function that creates the required pointer aliases, and type() method + * + * Usage: + * @code{.cpp} + * class Derived : public Graph + * { + * public: + * FUSE_GRAPH_DEFINITIONS(Derived) + * // The rest of the derived graph implementation + * } + * @endcode + */ +#define FUSE_GRAPH_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -210,7 +209,7 @@ class Graph * @param[in] constraint_uuid The UUID of the constraint being searched for * @return True if this constraint already exists, False otherwise */ - virtual bool constraintExists(const UUID & constraint_uuid) const = 0; + virtual bool constraintExists(const UUID& constraint_uuid) const = 0; /** * @brief Add a new constraint to the graph @@ -230,7 +229,7 @@ class Graph * @param[in] constraint_uuid The UUID of the constraint to be removed * @return True if the constraint was removed, false otherwise */ - virtual bool removeConstraint(const UUID & constraint_uuid) = 0; + virtual bool removeConstraint(const UUID& constraint_uuid) = 0; /** * @brief Read-only access to a constraint from the graph by UUID @@ -240,7 +239,7 @@ class Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - virtual const Constraint & getConstraint(const UUID & constraint_uuid) const = 0; + virtual const Constraint& getConstraint(const UUID& constraint_uuid) const = 0; /** * @brief Read-only access to all of the constraints in the graph @@ -257,7 +256,7 @@ class Graph * @return A read-only iterator range containing all constraints that involve the specified * variable */ - virtual const_constraint_range getConnectedConstraints(const UUID & variable_uuid) const = 0; + virtual const_constraint_range getConnectedConstraints(const UUID& variable_uuid) const = 0; /** * @brief Check if the variable already exists in the graph @@ -265,7 +264,7 @@ class Graph * @param[in] variable_uuid The UUID of the variable being searched for * @return True if this variable already exists, False otherwise */ - virtual bool variableExists(const UUID & variable_uuid) const = 0; + virtual bool variableExists(const UUID& variable_uuid) const = 0; /** * @brief Add a new variable to the graph @@ -284,7 +283,7 @@ class Graph * @param[in] variable_uuid The UUID of the variable to be removed * @return True if the variable was removed, false otherwise */ - virtual bool removeVariable(const UUID & variable_uuid) = 0; + virtual bool removeVariable(const UUID& variable_uuid) = 0; /** * @brief Read-only access to a variable in the graph by UUID @@ -294,7 +293,7 @@ class Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - virtual const Variable & getVariable(const UUID & variable_uuid) const = 0; + virtual const Variable& getVariable(const UUID& variable_uuid) const = 0; /** * @brief Read-only access to all of the variables in the graph @@ -311,7 +310,7 @@ class Graph * @return A read-only iterator range containing all variables that involve the specified * constraint */ - virtual const_variable_range getConnectedVariables(const UUID & constraint_uuid) const; + virtual const_variable_range getConnectedVariables(const UUID& constraint_uuid) const; /** * @brief Configure a variable to hold its current value constant during optimization @@ -325,7 +324,7 @@ class Graph * optimization, or if the variable's value is allowed to change during * optimization. */ - virtual void holdVariable(const UUID & variable_uuid, bool hold_constant = true) = 0; + virtual void holdVariable(const UUID& variable_uuid, bool hold_constant = true) = 0; /** * @brief Check whether a variable is on hold or not @@ -333,7 +332,7 @@ class Graph * @param[in] variable_uuid The variable to test * @return True if the variable is on hold, false otherwise */ - virtual bool isVariableOnHold(const UUID & variable_uuid) const = 0; + virtual bool isVariableOnHold(const UUID& variable_uuid) const = 0; /** * @brief Compute the marginal covariance blocks for the requested set of variable pairs. @@ -353,18 +352,17 @@ class Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - virtual void getCovariance( - const std::vector> & covariance_requests, - std::vector> & covariance_matrices, - const ceres::Covariance::Options & options = ceres::Covariance::Options(), - const bool use_tangent_space = true) const = 0; + virtual void getCovariance(const std::vector>& covariance_requests, + std::vector>& covariance_matrices, + const ceres::Covariance::Options& options = ceres::Covariance::Options(), + const bool use_tangent_space = true) const = 0; /** * @brief Update the graph with the contents of a transaction * * @param[in] transaction A set of variable and constraints additions and deletions */ - void update(const Transaction & transaction); + void update(const Transaction& transaction); /** * @brief Optimize the values of the current set of variables, given the current set of @@ -378,8 +376,7 @@ class Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - virtual ceres::Solver::Summary optimize( - const ceres::Solver::Options & options = ceres::Solver::Options()) = 0; + virtual ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) = 0; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -396,11 +393,11 @@ class Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - virtual ceres::Solver::Summary optimizeFor( - const rclcpp::Duration & max_optimization_time, - const ceres::Solver::Options & options = ceres::Solver::Options(), - rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0; // NOTE(CH3): We need to copy - // because clock.now() is non-const + virtual ceres::Solver::Summary + optimizeFor(const rclcpp::Duration& max_optimization_time, + const ceres::Solver::Options& options = ceres::Solver::Options(), + rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0; // NOTE(CH3): We need to copy + // because clock.now() is non-const /** * @brief Evaluate the values of the current set of variables, given the current set of @@ -425,20 +422,18 @@ class Graph * solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401 * @return True if the problem evaluation was successful; False, otherwise. */ - virtual bool evaluate( - double * cost, std::vector * residuals = nullptr, - std::vector * gradient = nullptr, - const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const = 0; + virtual bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, + const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const = 0; /** * @brief Structure containing the cost and residual information for a single constraint. */ struct ConstraintCost { - double cost {}; //!< The pre-loss-function cost of the constraint, computed as the norm of the - //!< residuals - double loss {}; //!< The final cost of the constraint after any loss functions have been - //!< applied + double cost{}; //!< The pre-loss-function cost of the constraint, computed as the norm of the + //!< residuals + double loss{}; //!< The final cost of the constraint after any loss functions have been + //!< applied std::vector residuals; //!< The individual residuals for the constraint }; @@ -451,18 +446,15 @@ class Graph * @param[in] last An iterator pointing to one passed the last UUID of the desired constraints * @param[out] output An output iterator capable of assignment to a ConstraintCost object */ - template - void getConstraintCosts( - UuidForwardIterator first, - UuidForwardIterator last, - OutputIterator output); + template + void getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output); /** * @brief Print a human-readable description of the graph 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; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Serialize this graph into the provided binary archive @@ -474,7 +466,7 @@ class Graph * * @param[out] archive - The archive to serialize this graph into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this graph into the provided text archive @@ -486,7 +478,7 @@ class Graph * * @param[out] archive - The archive to serialize this graph into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this graph @@ -498,7 +490,7 @@ class Graph * * @param[in] archive - The archive holding serialized graph data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this graph @@ -510,7 +502,7 @@ class Graph * * @param[in] archive - The archive holding serialized graph data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: // Allow Boost Serialization access to private methods @@ -529,8 +521,8 @@ class Graph * @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 */) + template + void serialize(Archive& /* archive */, const unsigned int /* version */) { } }; @@ -538,27 +530,25 @@ class Graph /** * Stream operator for printing Graph objects. */ -std::ostream & operator<<(std::ostream & stream, const Graph & graph); +std::ostream& operator<<(std::ostream& stream, const Graph& graph); - -template -void Graph::getConstraintCosts( - UuidForwardIterator first, - UuidForwardIterator last, - OutputIterator output) +template +void Graph::getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output) { // @todo(swilliams) When I eventually refactor the Graph class to implement more of the // requirements in the base class, it should be possible to make better use of // the Problem object and avoid creating and deleting the cost and loss // functions. - while (first != last) { + while (first != last) + { // Get the next requested constraint - const auto & constraint = getConstraint(*first); + const auto& constraint = getConstraint(*first); // Collect all of the involved variables - auto parameter_blocks = std::vector(); + auto parameter_blocks = std::vector(); parameter_blocks.reserve(constraint.variables().size()); - for (auto variable_uuid : constraint.variables()) { - const auto & variable = getVariable(variable_uuid); + for (auto variable_uuid : constraint.variables()) + { + const auto& variable = getVariable(variable_uuid); parameter_blocks.push_back(variable.data()); } // Compute the residuals for this constraint using the cost function @@ -568,18 +558,18 @@ void Graph::getConstraintCosts( cost_function->Evaluate(parameter_blocks.data(), cost.residuals.data(), nullptr); // Compute the combined cost cost.cost = - std::sqrt( - std::inner_product( - cost.residuals.begin(), cost.residuals.end(), - cost.residuals.begin(), 0.0)); + std::sqrt(std::inner_product(cost.residuals.begin(), cost.residuals.end(), cost.residuals.begin(), 0.0)); // Apply the loss function, if one is configured auto loss_function = std::unique_ptr(constraint.lossFunction()); - if (loss_function) { + if (loss_function) + { double loss_result[3]; // The Loss function returns the loss-adjusted cost plus the first and // second derivative loss_function->Evaluate(cost.cost, loss_result); cost.loss = loss_result[0]; - } else { + } + else + { cost.loss = cost.cost; } // Add the final cost to the output diff --git a/fuse_core/include/fuse_core/graph_deserializer.hpp b/fuse_core/include/fuse_core/graph_deserializer.hpp index eadd32462..44f61177d 100644 --- a/fuse_core/include/fuse_core/graph_deserializer.hpp +++ b/fuse_core/include/fuse_core/graph_deserializer.hpp @@ -46,7 +46,7 @@ namespace fuse_core /** * @brief Serialize a graph into a message */ -void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGraph & msg); +void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg); /** * @brief Deserialize a graph @@ -73,8 +73,7 @@ class GraphDeserializer * @param[in] msg The SerializedGraph message to be deserialized * @return A unique_ptr to a derived Graph object */ - inline fuse_core::Graph::UniquePtr deserialize( - const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const + inline fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const { return deserialize(*msg); } @@ -88,7 +87,7 @@ class GraphDeserializer * @param[in] msg The SerializedGraph message to be deserialized * @return A unique_ptr to a derived Graph object */ - fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph & msg) const; + fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/local_parameterization.hpp b/fuse_core/include/fuse_core/local_parameterization.hpp index c6cebcc03..9df7a40dc 100644 --- a/fuse_core/include/fuse_core/local_parameterization.hpp +++ b/fuse_core/include/fuse_core/local_parameterization.hpp @@ -99,11 +99,8 @@ class LocalParameterization * @param[in] x variable of size \p GlobalSize() * @param[in] delta variable of size \p LocalSize() * @param[out] x_plus_delta of size \p GlobalSize() - */ - virtual bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const = 0; + */ + virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; /** * @brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0. @@ -112,7 +109,7 @@ class LocalParameterization * @param[out] jacobian a row-major GlobalSize() x LocalSize() matrix. * @return */ - virtual bool ComputeJacobian(const double * x, double * jacobian) const = 0; + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -129,10 +126,7 @@ class LocalParameterization * \p LocalSize() * @return True if successful, false otherwise */ - virtual bool Minus( - const double * x, - const double * y, - double * y_minus_x) const = 0; + 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 x == y @@ -142,9 +136,7 @@ class LocalParameterization * GlobalSize() * @return True if successful, false otherwise */ - virtual bool ComputeMinusJacobian( - const double * x, - double * jacobian) const = 0; + virtual bool ComputeMinusJacobian(const double* x, double* jacobian) const = 0; #if CERES_SUPPORTS_MANIFOLDS // If the fuse::LocalParameterization class does not inherit from the @@ -164,27 +156,25 @@ class LocalParameterization * @param[in] global_matrix is a num_rows x GlobalSize row major matrix. * @param[out] local_matrix is a num_rows x LocalSize row major matrix. */ - virtual bool MultiplyByJacobian( - const double * x, - const int num_rows, - const double * global_matrix, - double * local_matrix) const + virtual bool MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, + double* local_matrix) const { using Matrix = Eigen::Matrix; using MatrixRef = Eigen::Map; using ConstMatrixRef = Eigen::Map; - if (LocalSize() == 0) { + if (LocalSize() == 0) + { return true; } Matrix jacobian(GlobalSize(), LocalSize()); - if (!ComputeJacobian(x, jacobian.data())) { + if (!ComputeJacobian(x, jacobian.data())) + { return false; } - MatrixRef(local_matrix, num_rows, LocalSize()) = - ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + MatrixRef(local_matrix, num_rows, LocalSize()) = ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; return true; } #endif @@ -200,8 +190,10 @@ 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 - void serialize(Archive & /* archive */, const unsigned int /* version */) {} + template + void serialize(Archive& /* archive */, const unsigned int /* version */) + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index 8465e574f..b20cbda75 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -58,10 +58,10 @@ * } * @endcode */ -#define FUSE_LOSS_CLONE_DEFINITION(...) \ - fuse_core::Loss::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_LOSS_CLONE_DEFINITION(...) \ + fuse_core::Loss::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -77,22 +77,22 @@ * } * @endcode */ -#define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -110,17 +110,17 @@ * } * @endcode */ -#define FUSE_LOSS_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_LOSS_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** @@ -137,13 +137,12 @@ * } * @endcode */ -#define FUSE_LOSS_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_LOSS_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -174,7 +173,7 @@ class Loss FUSE_SMART_PTR_ALIASES_ONLY(Loss) static constexpr ceres::Ownership Ownership = - ceres::Ownership::TAKE_OWNERSHIP; //!< The ownership of the ceres::LossFunction* returned by + ceres::Ownership::TAKE_OWNERSHIP; //!< The ownership of the ceres::LossFunction* returned by //!< lossFunction() /** @@ -197,12 +196,10 @@ class Loss * server. */ virtual void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) = 0; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) = 0; /** * @brief Returns a unique name for this loss function type. @@ -217,7 +214,7 @@ class Loss * * @param stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -230,7 +227,7 @@ class Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - virtual ceres::LossFunction * lossFunction() const = 0; + virtual ceres::LossFunction* lossFunction() const = 0; /** * @brief Perform a deep copy of the Loss and return a unique pointer to the copy @@ -254,7 +251,7 @@ class Loss * * @param[out] archive - The archive to serialize this loss function into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Loss into the provided text archive @@ -266,7 +263,7 @@ class Loss * * @param[out] archive - The archive to serialize this loss function into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Loss @@ -278,7 +275,7 @@ class Loss * * @param[in] archive - The archive holding serialized Loss data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Loss @@ -290,7 +287,7 @@ class Loss * * @param[in] archive - The archive holding serialized Loss data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: // Allow Boost Serialization access to private methods @@ -303,8 +300,8 @@ class Loss * @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 */) + template + void serialize(Archive& /* archive */, const unsigned int /* version */) { } }; @@ -312,7 +309,7 @@ class Loss /** * Stream operator implementation used for all derived Loss classes. */ -std::ostream & operator<<(std::ostream & stream, const Loss & loss); +std::ostream& operator<<(std::ostream& stream, const Loss& loss); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss_loader.hpp b/fuse_core/include/fuse_core/loss_loader.hpp index 0a9b86d39..5fd4242b5 100644 --- a/fuse_core/include/fuse_core/loss_loader.hpp +++ b/fuse_core/include/fuse_core/loss_loader.hpp @@ -58,17 +58,17 @@ class LossLoader * * @return Loss loader singleton instance */ - static LossLoader & getInstance() + static LossLoader& getInstance() { static LossLoader instance; return instance; } // Delete copy and move constructors and assign operators - LossLoader(const LossLoader &) = delete; - LossLoader(LossLoader &&) = delete; - LossLoader & operator=(const LossLoader &) = delete; - LossLoader & operator=(LossLoader &&) = delete; + LossLoader(const LossLoader&) = delete; + LossLoader(LossLoader&&) = delete; + LossLoader& operator=(const LossLoader&) = delete; + LossLoader& operator=(LossLoader&&) = delete; /** * @brief Create unique instance of a loss function loaded with pluginlib @@ -76,7 +76,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ - pluginlib::UniquePtr createUniqueInstance(const std::string & lookup_name) + pluginlib::UniquePtr createUniqueInstance(const std::string& lookup_name) { return loss_loader_.createUniqueInstance(lookup_name); } @@ -85,8 +85,7 @@ class LossLoader /** * @brief Constructor */ - LossLoader() - : loss_loader_("fuse_core", "fuse_core::Loss") + LossLoader() : loss_loader_("fuse_core", "fuse_core::Loss") { } @@ -100,7 +99,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ -inline pluginlib::UniquePtr createUniqueLoss(const std::string & lookup_name) +inline pluginlib::UniquePtr createUniqueLoss(const std::string& lookup_name) { return LossLoader::getInstance().createUniqueInstance(lookup_name); } diff --git a/fuse_core/include/fuse_core/macros.hpp b/fuse_core/include/fuse_core/macros.hpp index 3af81587a..1045af348 100644 --- a/fuse_core/include/fuse_core/macros.hpp +++ b/fuse_core/include/fuse_core/macros.hpp @@ -56,7 +56,8 @@ #define FUSE_CORE__MACROS_HPP_ /* *INDENT-OFF* */ -#pragma message("Including header and is deprecated, include instead.") /* NOLINT */ +#pragma message( \ + "Including header and is deprecated, include instead.") /* NOLINT */ /* *INDENT-ON* */ // Required by __MAKE_SHARED_ALIGNED_DEFINITION, that uses Eigen::aligned_allocator(). @@ -79,9 +80,9 @@ * definitions. */ #if __cpp_aligned_new - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() #else - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW #endif /** @@ -89,11 +90,11 @@ * * Use in the public section of the class. */ -#define SMART_PTR_DEFINITIONS(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ +#define SMART_PTR_DEFINITIONS(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** @@ -107,16 +108,15 @@ * Use in the public section of the class. */ #if __cpp_aligned_new - #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - SMART_PTR_DEFINITIONS(__VA_ARGS__) +#define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) SMART_PTR_DEFINITIONS(__VA_ARGS__) #else - #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -127,56 +127,49 @@ * * Use in the public section of the class. */ -#define SMART_PTR_ALIASES_ONLY(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ +#define SMART_PTR_ALIASES_ONLY(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ __UNIQUE_PTR_ALIAS(__VA_ARGS__) -#define __SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ +#define __SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ using ConstSharedPtr = std::shared_ptr; -#define __MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ +#define __MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args)...); \ } -#define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ +#define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>(Eigen::aligned_allocator<__VA_ARGS__>(), std::forward(args)...); \ } -#define __WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ +#define __WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ using ConstWeakPtr = std::weak_ptr; -#define __UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; +#define __UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L - #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } +#define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args)...); \ + } #else - #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } +#define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args)...)); \ + } #endif #endif // FUSE_CORE__MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/manifold.hpp b/fuse_core/include/fuse_core/manifold.hpp index d62b0da96..7173a6cdc 100644 --- a/fuse_core/include/fuse_core/manifold.hpp +++ b/fuse_core/include/fuse_core/manifold.hpp @@ -112,7 +112,7 @@ class Manifold : public ceres::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Plus(const double * x, const double * delta, double * x_plus_delta) const = 0; + virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; /** * @brief Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, @@ -125,7 +125,7 @@ class Manifold : public ceres::Manifold * matrix. * @return */ - virtual bool PlusJacobian(const double * x, double * jacobian) const = 0; + virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -141,7 +141,7 @@ class Manifold : public ceres::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Minus(const double * y, const double * x, double * y_minus_x) const = 0; + virtual bool Minus(const double* y, const double* x, double* y_minus_x) const = 0; /** * @brief Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e @@ -152,7 +152,7 @@ class Manifold : public ceres::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - virtual bool MinusJacobian(const double * x, double * jacobian) const = 0; + virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; private: // Allow Boost Serialization access to private methods @@ -167,8 +167,10 @@ class Manifold : public ceres::Manifold * @param[in] version - The version of the archive being read/written. * Generally unused. */ - template - void serialize(Archive & /* archive */, const unsigned int /* version */) {} + template + void serialize(Archive& /* archive */, const unsigned int /* version */) + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/manifold_adapter.hpp b/fuse_core/include/fuse_core/manifold_adapter.hpp index 730c02bc7..b85c87d6b 100644 --- a/fuse_core/include/fuse_core/manifold_adapter.hpp +++ b/fuse_core/include/fuse_core/manifold_adapter.hpp @@ -65,7 +65,7 @@ class ManifoldAdapter : public fuse_core::Manifold * * @param[in] local_parameterization Raw pointer to a derviced fuse::LocalParameterization object */ - explicit ManifoldAdapter(fuse_core::LocalParameterization * local_parameterization) + explicit ManifoldAdapter(fuse_core::LocalParameterization* local_parameterization) { local_parameterization_.reset(local_parameterization); } @@ -89,7 +89,10 @@ class ManifoldAdapter : public fuse_core::Manifold * * @return int Dimension of the ambient space in which the manifold is embedded. */ - int AmbientSize() const override {return local_parameterization_->GlobalSize();} + int AmbientSize() const override + { + return local_parameterization_->GlobalSize(); + } /** * @brief Dimension of the manifold/tangent space. @@ -98,7 +101,10 @@ class ManifoldAdapter : public fuse_core::Manifold * * @return int Dimension of the manifold/tangent space. */ - int TangentSize() const override {return local_parameterization_->LocalSize();} + int TangentSize() const override + { + return local_parameterization_->LocalSize(); + } /** * @brief x_plus_delta = Plus(x, delta), @@ -112,7 +118,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { return local_parameterization_->Plus(x, delta, x_plus_delta); } @@ -128,7 +134,7 @@ class ManifoldAdapter : public fuse_core::Manifold * matrix. * @return */ - bool PlusJacobian(const double * x, double * jacobian) const override + bool PlusJacobian(const double* x, double* jacobian) const override { return local_parameterization_->ComputeJacobian(x, jacobian); } @@ -147,7 +153,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { return local_parameterization_->Minus(x, y, y_minus_x); } @@ -161,7 +167,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - bool MinusJacobian(const double * x, double * jacobian) const override + bool MinusJacobian(const double* x, double* jacobian) const override { return local_parameterization_->ComputeMinusJacobian(x, jacobian); } @@ -178,11 +184,11 @@ class ManifoldAdapter : public fuse_core::Manifold * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & local_parameterization_; + archive& boost::serialization::base_object(*this); + archive& local_parameterization_; } /** diff --git a/fuse_core/include/fuse_core/message_buffer.hpp b/fuse_core/include/fuse_core/message_buffer.hpp index f05fc8744..11dde62a4 100644 --- a/fuse_core/include/fuse_core/message_buffer.hpp +++ b/fuse_core/include/fuse_core/message_buffer.hpp @@ -56,7 +56,7 @@ namespace fuse_core * * It is assumed that all messages are received sequentially. */ -template +template class MessageBuffer { public: @@ -70,8 +70,7 @@ class MessageBuffer * method for directly accessing the first member. When dereferenced, an iterator returns a * std::pair&. */ - using message_range = boost::any_range, - boost::forward_traversal_tag>; + using message_range = boost::any_range, boost::forward_traversal_tag>; /** * @brief A range of timestamps @@ -90,7 +89,7 @@ class MessageBuffer * timestamps that are older than the buffer length, an exception will be * thrown. */ - explicit MessageBuffer(const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + explicit MessageBuffer(const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -100,7 +99,7 @@ class MessageBuffer /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration & bufferLength() const + const rclcpp::Duration& bufferLength() const { return buffer_length_; } @@ -108,7 +107,7 @@ class MessageBuffer /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration & buffer_length) + void bufferLength(const rclcpp::Duration& buffer_length) { buffer_length_ = buffer_length; } @@ -122,7 +121,7 @@ class MessageBuffer * @param[in] stamp The stamp to assign to the message * @param[in] msg A message */ - void insert(const rclcpp::Time & stamp, const Message & msg); + void insert(const rclcpp::Time& stamp, const Message& msg); /** * @brief Query the buffer for the set of messages between two timestamps @@ -142,9 +141,7 @@ class MessageBuffer * @return An iterator range containing all of the messages between the * specified stamps. */ - message_range query( - const rclcpp::Time & beginning_stamp, const rclcpp::Time & ending_stamp, - bool extended_range = true); + message_range query(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, bool extended_range = true); /** * @brief Read-only access to the current set of timestamps @@ -155,7 +152,7 @@ class MessageBuffer protected: using Buffer = std::deque>; - Buffer buffer_; //!< The container of received messages, sorted by timestamp + Buffer buffer_; //!< The container of received messages, sorted by timestamp rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than //!< \p buffer_length_ will be removed from the motion model //!< history @@ -164,7 +161,7 @@ class MessageBuffer * @brief Helper function used with boost::transform_iterators to convert the internal Buffer * value type into a const rclcpp::Time& iterator compatible with stamp_range */ - static const rclcpp::Time & extractStamp(const typename Buffer::value_type & element) + static const rclcpp::Time& extractStamp(const typename Buffer::value_type& element) { return element.first; } diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp index 218f89ae0..1a3cdebb3 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.hpp +++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp @@ -46,106 +46,101 @@ namespace fuse_core { -template -MessageBuffer::MessageBuffer(const rclcpp::Duration & buffer_length) -: buffer_length_(buffer_length) +template +MessageBuffer::MessageBuffer(const rclcpp::Duration& buffer_length) : buffer_length_(buffer_length) { } -template -void MessageBuffer::insert(const rclcpp::Time & stamp, const Message & msg) +template +void MessageBuffer::insert(const rclcpp::Time& stamp, const Message& msg) { buffer_.emplace_back(stamp, msg); purgeHistory(); } -template -typename MessageBuffer::message_range MessageBuffer::query( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - bool extended_range) +template +typename MessageBuffer::message_range MessageBuffer::query(const rclcpp::Time& beginning_stamp, + const rclcpp::Time& ending_stamp, + bool extended_range) { // Verify the query is valid - if (ending_stamp < beginning_stamp) { + if (ending_stamp < beginning_stamp) + { std::stringstream beginning_time_ss; beginning_time_ss << beginning_stamp.seconds(); std::stringstream ending_time_ss; ending_time_ss << ending_stamp.seconds(); - throw std::invalid_argument( - "The beginning_stamp (" + beginning_time_ss.str() + ") must be less than or equal to " - "the ending_stamp (" + ending_time_ss.str() + ")."); + throw std::invalid_argument("The beginning_stamp (" + beginning_time_ss.str() + + ") must be less than or equal to " + "the ending_stamp (" + + ending_time_ss.str() + ")."); } // Verify the query is within the bounds of the buffer - if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || - (ending_stamp > buffer_.back().first)) + if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || (ending_stamp > buffer_.back().first)) { std::stringstream requested_time_range_ss; - requested_time_range_ss << "(" << beginning_stamp.seconds() << ", " << ending_stamp.seconds() << - ")"; + requested_time_range_ss << "(" << beginning_stamp.seconds() << ", " << ending_stamp.seconds() << ")"; std::stringstream available_time_range_ss; - if (buffer_.empty()) { + if (buffer_.empty()) + { available_time_range_ss << "(EMPTY)"; - } else { - available_time_range_ss << "(" << buffer_.front().first.seconds() << ", " - << buffer_.back().first.seconds() << ")"; } - throw std::out_of_range( - "The requested time range " + requested_time_range_ss.str() + - " is outside the available time range " + available_time_range_ss.str() + "."); + else + { + available_time_range_ss << "(" << buffer_.front().first.seconds() << ", " << buffer_.back().first.seconds() + << ")"; + } + throw std::out_of_range("The requested time range " + requested_time_range_ss.str() + + " is outside the available time range " + available_time_range_ss.str() + "."); } // Find the entry that is strictly greater than the requested beginning stamp. If the extended // range flag is true, we will then back up one entry. - auto upper_bound_comparison = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; - auto beginning_iter = std::upper_bound( - buffer_.begin(), - buffer_.end(), beginning_stamp, upper_bound_comparison); - if (extended_range) { + auto upper_bound_comparison = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto beginning_iter = std::upper_bound(buffer_.begin(), buffer_.end(), beginning_stamp, upper_bound_comparison); + if (extended_range) + { --beginning_iter; } // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is // false, we will back up one entry. - auto lower_bound_comparison = [](const auto & element, const auto & stamp) -> bool - { - return element.first < stamp; - }; - auto ending_iter = std::lower_bound( - buffer_.begin(), - buffer_.end(), ending_stamp, lower_bound_comparison); - if (extended_range && (ending_iter != buffer_.end())) { + auto lower_bound_comparison = [](const auto& element, const auto& stamp) -> bool { return element.first < stamp; }; + auto ending_iter = std::lower_bound(buffer_.begin(), buffer_.end(), ending_stamp, lower_bound_comparison); + if (extended_range && (ending_iter != buffer_.end())) + { ++ending_iter; } // Return the beginning and ending iterators as an iterator range with the correct deference type return message_range(beginning_iter, ending_iter); } -template +template typename MessageBuffer::stamp_range MessageBuffer::stamps() const { - return stamp_range( - boost::make_transform_iterator(buffer_.begin(), extractStamp), - boost::make_transform_iterator(buffer_.end(), extractStamp)); + return stamp_range(boost::make_transform_iterator(buffer_.begin(), extractStamp), + boost::make_transform_iterator(buffer_.end(), extractStamp)); } -template +template void MessageBuffer::purgeHistory() { // Purge any messages that are more than buffer_length_ seconds older than the most recent entry // A setting of rclcpp::Duration::max() means "keep everything" // And we want to keep at least two entries in buffer at all times, regardless of the stamps. - if ((buffer_length_ == rclcpp::Duration::max()) || (buffer_.size() <= 2)) { + if ((buffer_length_ == rclcpp::Duration::max()) || (buffer_.size() <= 2)) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = buffer_.back().first; + const auto& ending_stamp = buffer_.back().first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length_.seconds()) { + if (ending_stamp.seconds() > buffer_length_.seconds()) + { expiration_time = ending_stamp - buffer_length_; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -154,14 +149,10 @@ void MessageBuffer::purgeHistory() // Be careful to ensure that: // - at least two entries remains at all times // - the buffer covers *at least* until the expiration time. Longer is acceptable. - auto is_greater = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; - auto expiration_iter = std::upper_bound( - buffer_.begin(), - buffer_.end(), expiration_time, is_greater); - if (expiration_iter != buffer_.begin()) { + auto is_greater = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto expiration_iter = std::upper_bound(buffer_.begin(), buffer_.end(), expiration_time, is_greater); + if (expiration_iter != buffer_.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time buffer_.erase(buffer_.begin(), std::prev(expiration_iter)); diff --git a/fuse_core/include/fuse_core/motion_model.hpp b/fuse_core/include/fuse_core/motion_model.hpp index b833b0b33..7fdbc1746 100644 --- a/fuse_core/include/fuse_core/motion_model.hpp +++ b/fuse_core/include/fuse_core/motion_model.hpp @@ -71,7 +71,7 @@ class MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - virtual bool apply(Transaction & transaction) = 0; + virtual bool apply(Transaction& transaction) = 0; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -87,7 +87,9 @@ class MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) {} + virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required post-construction initialization, such as subscribing to topics or @@ -100,14 +102,13 @@ class MotionModel * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) = 0; + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) = 0; /** * @brief Get the unique name of this motion model */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -118,7 +119,9 @@ class MotionModel * the motion model to reset any internal state before the optimizer begins processing after * a reset. No calls to apply() will happen before the optimizer calls start(). */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -130,7 +133,9 @@ class MotionModel * internal state before the optimizer begins processing after a reset. No calls to apply() * will happen until start() has been called again. */ - virtual void stop() {} + virtual void stop() + { + } protected: /** diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp index 897e568ed..17fe3626b 100644 --- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -29,18 +29,11 @@ #include #include - -#define ALL_FUSE_CORE_NODE_INTERFACES \ - fuse_core::node_interfaces::Base, \ - fuse_core::node_interfaces::Clock, \ - fuse_core::node_interfaces::Graph, \ - fuse_core::node_interfaces::Logging, \ - fuse_core::node_interfaces::Parameters, \ - fuse_core::node_interfaces::Services, \ - fuse_core::node_interfaces::TimeSource, \ - fuse_core::node_interfaces::Timers, \ - fuse_core::node_interfaces::Topics, \ - fuse_core::node_interfaces::Waitables +#define ALL_FUSE_CORE_NODE_INTERFACES \ + fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Clock, fuse_core::node_interfaces::Graph, \ + fuse_core::node_interfaces::Logging, fuse_core::node_interfaces::Parameters, \ + fuse_core::node_interfaces::Services, fuse_core::node_interfaces::TimeSource, \ + fuse_core::node_interfaces::Timers, fuse_core::node_interfaces::Topics, fuse_core::node_interfaces::Waitables namespace fuse_core { @@ -57,7 +50,7 @@ using Timers = rclcpp::node_interfaces::NodeTimersInterface; using Topics = rclcpp::node_interfaces::NodeTopicsInterface; using Waitables = rclcpp::node_interfaces::NodeWaitablesInterface; -template +template using NodeInterfaces = ::rclcpp::node_interfaces::NodeInterfaces; } // namespace node_interfaces } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/parameter.hpp b/fuse_core/include/fuse_core/parameter.hpp index bab6bcecf..b4376e9f1 100644 --- a/fuse_core/include/fuse_core/parameter.hpp +++ b/fuse_core/include/fuse_core/parameter.hpp @@ -50,7 +50,7 @@ namespace fuse_core { // Helper function to get a namespace string with a '.' suffix, but only if not empty -std::string joinParameterName(const std::string & left, const std::string & right); +std::string joinParameterName(const std::string& left, const std::string& right); // NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces @@ -70,30 +70,34 @@ std::string joinParameterName(const std::string & left, const std::string & righ * @return The value of the parameter. * @throws rclcpp::exceptions::InvalidParameterTypeException if the parameter type does not match */ -template +template T getParam( - node_interfaces::NodeInterfaces interfaces, - const std::string & parameter_name, - const T & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), - bool ignore_override = false) + node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, + const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) { auto params_interface = interfaces.get_node_parameters_interface(); - if (params_interface->has_parameter(parameter_name)) { + if (params_interface->has_parameter(parameter_name)) + { return params_interface->get_parameter(parameter_name).get_parameter_value().get(); - } else { - try { - return params_interface->declare_parameter( - parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, ignore_override - ).get(); - } catch (const rclcpp::ParameterTypeException & ex) { + } + else + { + try + { + return params_interface + ->declare_parameter(parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, + ignore_override) + .get(); + } + catch (const rclcpp::ParameterTypeException& ex) + { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } } } - /** * @brief Compatibility wrapper for ros2 params in ros1 syntax * @@ -109,22 +113,23 @@ T getParam( * @return The value of the parameter. * @throws rclcpp::exceptions::InvalidParameterTypeException if the parameter type does not match */ -template +template T getParam( - node_interfaces::NodeInterfaces interfaces, - const std::string & parameter_name, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), - bool ignore_override = false) + node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) { // get advantage of parameter value template magic to get the correct rclcpp::ParameterType from T // NOTE(CH3): For the same reason we can't defer to the overload of getParam - rclcpp::ParameterValue value{T{}}; - try { - return interfaces.get_node_parameters_interface()->declare_parameter( - parameter_name, value.get_type(), parameter_descriptor, ignore_override - ).get(); - } catch (const rclcpp::ParameterTypeException & ex) { + rclcpp::ParameterValue value{ T{} }; + try + { + return interfaces.get_node_parameters_interface() + ->declare_parameter(parameter_name, value.get_type(), parameter_descriptor, ignore_override) + .get(); + } + catch (const rclcpp::ParameterTypeException& ex) + { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } } @@ -133,11 +138,9 @@ namespace detail { /** @brief Internal function for unit testing. * @internal -*/ + */ std::unordered_set -list_parameter_override_prefixes( - const std::map & overrides, - std::string prefix); +list_parameter_override_prefixes(const std::map& overrides, std::string prefix); } // namespace detail /** @@ -161,11 +164,10 @@ list_parameter_override_prefixes( * @param[in] prefix - the parameter prefix * @param[in] max_depth - how deep to return parameter override names, or 0 for * unlimited depth. -*/ + */ std::unordered_set -list_parameter_override_prefixes( - node_interfaces::NodeInterfaces interfaces, - std::string prefix); +list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, + std::string prefix); /** * @brief Utility method for handling required ROS params @@ -175,24 +177,18 @@ list_parameter_override_prefixes( * @param[out] value - The ROS parameter value for the \p key * @throws std::runtime_error if the parameter does not exist */ -inline -void getParamRequired( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & key, - std::string & value -) +inline void getParamRequired( + node_interfaces::NodeInterfaces + interfaces, + const std::string& key, std::string& value) { std::string default_value = ""; value = getParam(interfaces, key, default_value); - if (value == default_value) { - const std::string error = - "Could not find required parameter " + key + " in namespace " + - interfaces.get_node_base_interface()->get_namespace(); + if (value == default_value) + { + const std::string error = "Could not find required parameter " + key + " in namespace " + + interfaces.get_node_base_interface()->get_namespace(); RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), error); throw std::runtime_error(error); @@ -210,25 +206,19 @@ void getParamRequired( * @param[in] strict - Whether to check the loaded value is strictly positive or not, i.e. whether 0 * is accepted or not */ -template::value || std::is_floating_point::value>> -void getPositiveParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - T & default_value, - const bool strict = true -) +template ::value || std::is_floating_point::value>> +void getPositiveParam(node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, T& default_value, const bool strict = true) { T value = getParam(interfaces, parameter_name, default_value); - if (value < 0 || (strict && value == 0)) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "") - << " 0. Using the default value (" << default_value << ") instead."); - } else { + if (value < 0 || (strict && value == 0)) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "") + << " 0. Using the default value (" << default_value << ") instead."); + } + else + { default_value = value; } } @@ -243,13 +233,9 @@ void getPositiveParam( * @param[in] strict - Whether to check the loaded value is strictly positive or not, i.e. whether 0 * is accepted or not */ -inline void getPositiveParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - rclcpp::Duration & default_value, const bool strict = true) +inline void +getPositiveParam(node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, rclcpp::Duration& default_value, const bool strict = true) { double default_value_sec = default_value.seconds(); getPositiveParam(interfaces, parameter_name, default_value_sec, strict); @@ -270,15 +256,10 @@ inline void getPositiveParam( * parameter name does not exist * @return The loaded (or default) covariance matrix, generated from the diagonal vector */ -template +template fuse_core::Matrix getCovarianceDiagonalParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - Scalar default_value -) + node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, Scalar default_value) { using Vector = typename Eigen::Matrix; @@ -286,19 +267,16 @@ fuse_core::Matrix getCovarianceDiagonalParam( diagonal = getParam(interfaces, parameter_name, diagonal); const auto diagonal_size = diagonal.size(); - if (diagonal_size != Size) { - throw std::invalid_argument( - "Invalid size of " + std::to_string(diagonal_size) + ", expected " + - std::to_string(Size)); + if (diagonal_size != Size) + { + throw std::invalid_argument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + + std::to_string(Size)); } - if (std::any_of( - diagonal.begin(), diagonal.end(), - [](const auto & value) {return value < Scalar(0);})) // NOLINT(whitespace/braces) + if (std::any_of(diagonal.begin(), diagonal.end(), + [](const auto& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) { - throw std::invalid_argument( - "Invalid negative diagonal values in " + - fuse_core::to_string(Vector(diagonal.data()))); + throw std::invalid_argument("Invalid negative diagonal values in " + fuse_core::to_string(Vector(diagonal.data()))); } return Vector(diagonal.data()).asDiagonal(); @@ -312,16 +290,11 @@ fuse_core::Matrix getCovarianceDiagonalParam( * @return Loss function or nullptr if the parameter does not exist */ inline fuse_core::Loss::SharedPtr loadLossConfig( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & name -) + node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { - if (!interfaces.get_node_parameters_interface()->has_parameter( - name + ".type")) + if (!interfaces.get_node_parameters_interface()->has_parameter(name + ".type")) { return {}; } diff --git a/fuse_core/include/fuse_core/publisher.hpp b/fuse_core/include/fuse_core/publisher.hpp index 113b94605..6690b4fd0 100644 --- a/fuse_core/include/fuse_core/publisher.hpp +++ b/fuse_core/include/fuse_core/publisher.hpp @@ -84,14 +84,13 @@ class Publisher * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) = 0; + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) = 0; /** * @brief Get the unique name of this publisher */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Notify the publisher that an optimization cycle is complete, and about changes to the @@ -120,7 +119,9 @@ class Publisher * the publisher to reset any internal state before the optimizer begins processing after a * reset. No calls to notify() will happen before the optimizer calls start(). */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -132,7 +133,9 @@ class Publisher * internal state before the optimizer begins processing after a reset. No calls to notify() * will happen until start() has been called again. */ - virtual void stop() {} + virtual void stop() + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index 11f5a9639..935f0ea24 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -48,7 +48,7 @@ namespace fuse_core * @brief The signature of the callback function that will be executed for every generated * transaction object. */ -using TransactionCallback = std::function; +using TransactionCallback = std::function; /** * @brief The interface definition for sensor model plugins in the fuse ecosystem. @@ -91,29 +91,29 @@ class SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) {} + virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) + { + } /** - * @brief Perform any required post-construction initialization, such as subscribing to topics or - * reading from the parameter server. - * - * This will be called on each plugin after construction, and after the ROS node has been - * initialized. Plugins are encouraged to subnamespace any of their parameters to prevent - * conflicts and allow the same plugin to be used multiple times with different settings and - * topics. - * - * @param[in] name A unique name to give this plugin instance - * @param[in] transaction_callback The function to call every time a transaction is published - */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) = 0; + * @brief Perform any required post-construction initialization, such as subscribing to topics or + * reading from the parameter server. + * + * This will be called on each plugin after construction, and after the ROS node has been + * initialized. Plugins are encouraged to subnamespace any of their parameters to prevent + * conflicts and allow the same plugin to be used multiple times with different settings and + * topics. + * + * @param[in] name A unique name to give this plugin instance + * @param[in] transaction_callback The function to call every time a transaction is published + */ + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name, TransactionCallback transaction_callback) = 0; /** * @brief Get the unique name of this sensor */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -126,7 +126,9 @@ class SensorModel * * The sensor model must not send any transactions to the optimizer before start() is called. */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -139,7 +141,9 @@ class SensorModel * * The sensor model must not send any transactions to the optimizer after stop() is called. */ - virtual void stop() {} + virtual void stop() + { + } protected: /** diff --git a/fuse_core/include/fuse_core/serialization.hpp b/fuse_core/include/fuse_core/serialization.hpp index a6e1b21a4..9a73f804f 100644 --- a/fuse_core/include/fuse_core/serialization.hpp +++ b/fuse_core/include/fuse_core/serialization.hpp @@ -77,12 +77,12 @@ class MessageBufferStreamSource * * @param[in] data A byte vector from a ROS message */ - explicit MessageBufferStreamSource(const std::vector & data); + explicit MessageBufferStreamSource(const std::vector& data); /** * @brief The stream source is non-copyable */ - MessageBufferStreamSource operator=(const MessageBufferStreamSource &) = delete; + MessageBufferStreamSource operator=(const MessageBufferStreamSource&) = delete; /** * @brief Read up to n characters from the data vector @@ -93,11 +93,11 @@ class MessageBufferStreamSource * @param[in] n The number of bytes to read from the stream * @return The number of bytes read, or -1 to indicate EOF */ - std::streamsize read(char_type * s, std::streamsize n); + std::streamsize read(char_type* s, std::streamsize n); private: - const std::vector & data_; //!< Reference to the source container - size_t index_; //!< The next vector index to read + const std::vector& data_; //!< Reference to the source container + size_t index_; //!< The next vector index to read }; /** @@ -117,12 +117,12 @@ class MessageBufferStreamSink * * @param[in] data A byte vector from a ROS message */ - explicit MessageBufferStreamSink(std::vector & data); + explicit MessageBufferStreamSink(std::vector& data); /** * @brief The stream sink is non-copyable */ - MessageBufferStreamSink operator=(const MessageBufferStreamSink &) = delete; + MessageBufferStreamSink operator=(const MessageBufferStreamSink&) = delete; /** * @brief Write n characters to the data vector @@ -133,10 +133,10 @@ class MessageBufferStreamSink * @param[in] n The number of bytes to write to the stream * @return The number of bytes written */ - std::streamsize write(const char_type * s, std::streamsize n); + std::streamsize write(const char_type* s, std::streamsize n); private: - std::vector & data_; //!< Reference to the destination container + std::vector& data_; //!< Reference to the destination container }; } // namespace fuse_core @@ -149,13 +149,13 @@ namespace serialization /** * @brief Serialize a rclcpp::Time variable using Boost Serialization */ -template -void serialize(Archive & archive, rclcpp::Time & stamp, const unsigned int /* version */) +template +void serialize(Archive& archive, rclcpp::Time& stamp, const unsigned int /* version */) { auto nanoseconds = stamp.nanoseconds(); auto clock_type = stamp.get_clock_type(); - archive & nanoseconds; - archive & clock_type; + archive& nanoseconds; + archive& clock_type; stamp = rclcpp::Time(nanoseconds, clock_type); } @@ -165,21 +165,21 @@ void serialize(Archive & archive, rclcpp::Time & stamp, const unsigned int /* ve * https://stackoverflow.com/questions/54534047/eigen-matrix- * boostserialization-c17/54535484#54535484 */ -template -inline void serialize( - Archive & archive, - Eigen::Matrix & matrix, - const unsigned int /* version */) +template +inline void serialize(Archive& archive, Eigen::Matrix& matrix, + const unsigned int /* version */) { Eigen::Index rows = matrix.rows(); Eigen::Index cols = matrix.cols(); - archive & rows; - archive & cols; - if (rows != matrix.rows() || cols != matrix.cols()) { + archive& rows; + archive& cols; + if (rows != matrix.rows() || cols != matrix.cols()) + { matrix.resize(rows, cols); } - if (matrix.size() != 0) { - archive & boost::serialization::make_array(matrix.data(), rows * cols); + if (matrix.size() != 0) + { + archive& boost::serialization::make_array(matrix.data(), rows * cols); } } diff --git a/fuse_core/include/fuse_core/throttled_callback.hpp b/fuse_core/include/fuse_core/throttled_callback.hpp index 5afa1bbea..958229794 100644 --- a/fuse_core/include/fuse_core/throttled_callback.hpp +++ b/fuse_core/include/fuse_core/throttled_callback.hpp @@ -52,7 +52,7 @@ namespace fuse_core * * @tparam Callback The std::function callback */ -template +template class ThrottledCallback { public: @@ -67,23 +67,20 @@ class ThrottledCallback * throttling * @param[in] clock The clock to throttle against. Defaults to using RCL_SYSTEM_TIME */ - ThrottledCallback( - Callback && keep_callback = nullptr, // NOLINT(whitespace/operators) - Callback && drop_callback = nullptr, // NOLINT(whitespace/operators) - const rclcpp::Duration & throttle_period = rclcpp::Duration(0, 0), - rclcpp::Clock::SharedPtr clock = std::make_shared()) - : keep_callback_(keep_callback) - , drop_callback_(drop_callback) - , throttle_period_(throttle_period) - , clock_(clock) - {} + ThrottledCallback(Callback&& keep_callback = nullptr, // NOLINT(whitespace/operators) + Callback&& drop_callback = nullptr, // NOLINT(whitespace/operators) + const rclcpp::Duration& throttle_period = rclcpp::Duration(0, 0), + rclcpp::Clock::SharedPtr clock = std::make_shared()) + : keep_callback_(keep_callback), drop_callback_(drop_callback), throttle_period_(throttle_period), clock_(clock) + { + } /** * @brief Throttle period getter * * @return The current throttle period duration in seconds being used */ - const rclcpp::Duration & getThrottlePeriod() const + const rclcpp::Duration& getThrottlePeriod() const { return throttle_period_; } @@ -106,7 +103,7 @@ class ThrottledCallback * * @param[in] throttle_period The new throttle period duration in seconds to use */ - void setThrottlePeriod(const rclcpp::Duration & throttle_period) + void setThrottlePeriod(const rclcpp::Duration& throttle_period) { throttle_period_ = throttle_period; } @@ -116,7 +113,7 @@ class ThrottledCallback * * @param[in] keep_callback The new keep callback to use */ - void setKeepCallback(const Callback & keep_callback) + void setKeepCallback(const Callback& keep_callback) { keep_callback_ = keep_callback; } @@ -126,7 +123,7 @@ class ThrottledCallback * * @param[in] drop_callback The new drop callback to use */ - void setDropCallback(const Callback & drop_callback) + void setDropCallback(const Callback& drop_callback) { drop_callback_ = drop_callback; } @@ -136,7 +133,7 @@ class ThrottledCallback * * @return The last time the keep callback was called */ - const rclcpp::Time & getLastCalledTime() const + const rclcpp::Time& getLastCalledTime() const { return last_called_time_; } @@ -147,8 +144,8 @@ class ThrottledCallback * * @param[in] args The input arguments */ - template - void callback(Args &&... args) + template + void callback(Args&&... args) { // Keep the callback if: // @@ -158,20 +155,25 @@ class ThrottledCallback // (c) The elpased time between now and the last called time is greater than the throttle period rclcpp::Time now = clock_->now(); - if ((last_called_time_.nanoseconds() == 0) || - (throttle_period_.nanoseconds() == 0) || - now - last_called_time_ > throttle_period_) + if ((last_called_time_.nanoseconds() == 0) || (throttle_period_.nanoseconds() == 0) || + now - last_called_time_ > throttle_period_) { - if (keep_callback_) { + if (keep_callback_) + { keep_callback_(std::forward(args)...); } - if (last_called_time_.nanoseconds() == 0) { + if (last_called_time_.nanoseconds() == 0) + { last_called_time_ = now; - } else { + } + else + { last_called_time_ += throttle_period_; } - } else if (drop_callback_) { + } + else if (drop_callback_) + { drop_callback_(std::forward(args)...); } } @@ -181,18 +183,18 @@ class ThrottledCallback * * @param[in] args The input arguments */ - template - void operator()(Args &&... args) + template + void operator()(Args&&... args) { callback(std::forward(args)...); } private: - Callback keep_callback_; //!< The callback to call when kept, i.e. not dropped - Callback drop_callback_; //!< The callback to call when dropped because of throttling + Callback keep_callback_; //!< The callback to call when kept, i.e. not dropped + Callback drop_callback_; //!< The callback to call when dropped because of throttling rclcpp::Duration throttle_period_; //!< The throttling period duration in seconds - rclcpp::Clock::SharedPtr clock_; //!< The clock to throttle against - rclcpp::Time last_called_time_; //!< The last time the keep callback was called + rclcpp::Clock::SharedPtr clock_; //!< The clock to throttle against + rclcpp::Time last_called_time_; //!< The last time the keep callback was called }; /** @@ -200,9 +202,8 @@ class ThrottledCallback * * @tparam M The ROS message type */ -template -using ThrottledMessageCallback = - ThrottledCallback>; +template +using ThrottledMessageCallback = ThrottledCallback>; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/timestamp_manager.hpp b/fuse_core/include/fuse_core/timestamp_manager.hpp index 413ed6b9f..758395270 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.hpp +++ b/fuse_core/include/fuse_core/timestamp_manager.hpp @@ -85,10 +85,9 @@ class TimestampManager * ending_stamp. The variables should include initial values for the * optimizer. */ - using MotionModelFunction = std::function & constraints, - std::vector & variables)>; + using MotionModelFunction = + std::function& constraints, std::vector& variables)>; /** * @brief A range of timestamps @@ -110,9 +109,8 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - explicit TimestampManager( - MotionModelFunction generator, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + explicit TimestampManager(MotionModelFunction generator, + const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Constructor that accepts the motion model generator as a member function pointer and @@ -128,15 +126,11 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - template - TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables), - T * obj, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + template + TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables), + T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Constructor that accepts the motion model generator as a const member function pointer @@ -152,15 +146,11 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - template - TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) const, - T * obj, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + template + TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) const, + T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -170,7 +160,7 @@ class TimestampManager /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration & bufferLength() const + const rclcpp::Duration& bufferLength() const { return buffer_length_; } @@ -178,7 +168,7 @@ class TimestampManager /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration & buffer_length) + void bufferLength(const rclcpp::Duration& buffer_length) { buffer_length_ = buffer_length; } @@ -211,7 +201,7 @@ class TimestampManager * @throws std::invalid_argument If timestamps are not within the defined buffer length of the * motion model */ - void query(Transaction & transaction, bool update_variables = false); + void query(Transaction& transaction, bool update_variables = false); /** * @brief Read-only access to the current set of timestamps @@ -232,20 +222,14 @@ class TimestampManager std::vector variables; explicit MotionModelSegment(rcl_clock_type_t clock_t) - : beginning_stamp{0, 0, clock_t}, - ending_stamp{0, 0, clock_t} + : beginning_stamp{ 0, 0, clock_t }, ending_stamp{ 0, 0, clock_t } { } - MotionModelSegment( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - const std::vector & constraints, - const std::vector & variables) - : beginning_stamp(beginning_stamp), - ending_stamp(ending_stamp), - constraints(constraints), - variables(variables) + MotionModelSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + const std::vector& constraints, + const std::vector& variables) + : beginning_stamp(beginning_stamp), ending_stamp(ending_stamp), constraints(constraints), variables(variables) { } }; @@ -259,11 +243,11 @@ class TimestampManager */ using MotionModelHistory = std::map; - MotionModelFunction generator_; //!< Users upplied function that generates motion model - //!< constraints - rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than - //!< \p buffer_length_ will be removed from the motion model - //!< history + MotionModelFunction generator_; //!< Users upplied function that generates motion model + //!< constraints + rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than + //!< \p buffer_length_ will be removed from the motion model + //!< history MotionModelHistory motion_model_history_; //!< Container that stores all previously generated //!< motion models @@ -277,10 +261,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * addSegment */ - void addSegment( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - Transaction & transaction); + void addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, Transaction& transaction); /** * @brief Remove an existing MotionModelSegment, updating the provided transaction. @@ -292,9 +273,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * removeSegment */ - void removeSegment( - MotionModelHistory::iterator & iter, - Transaction & transaction); + void removeSegment(MotionModelHistory::iterator& iter, Transaction& transaction); /** * @brief Split an existing MotionModelSegment into two pieces at the provided timestamp, updating @@ -308,10 +287,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * splitSegment */ - void splitSegment( - MotionModelHistory::iterator & iter, - const rclcpp::Time & stamp, - Transaction & transaction); + void splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, Transaction& transaction); /** * @brief Remove any motion model segments that are older than \p buffer_length_ @@ -319,41 +295,25 @@ class TimestampManager void purgeHistory(); }; -template -TimestampManager::TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables), - T * obj, - const rclcpp::Duration & buffer_length) -: TimestampManager(std::bind(fp, - obj, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - buffer_length) +template +TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables), + T* obj, const rclcpp::Duration& buffer_length) + : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + buffer_length) { } -template -TimestampManager::TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) const, - T * obj, - const rclcpp::Duration & buffer_length) -: TimestampManager(std::bind(fp, - obj, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - buffer_length) +template +TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) const, + T* obj, const rclcpp::Duration& buffer_length) + : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + buffer_length) { } diff --git a/fuse_core/include/fuse_core/transaction.hpp b/fuse_core/include/fuse_core/transaction.hpp index 514536f40..4449c439d 100644 --- a/fuse_core/include/fuse_core/transaction.hpp +++ b/fuse_core/include/fuse_core/transaction.hpp @@ -111,19 +111,28 @@ class Transaction /** * @brief Read-only access to this transaction's timestamp */ - const rclcpp::Time & stamp() const {return stamp_;} + const rclcpp::Time& stamp() const + { + return stamp_; + } /** * @brief Write access to this transaction's timestamp */ - void stamp(const rclcpp::Time & stamp) {stamp_ = stamp;} + void stamp(const rclcpp::Time& stamp) + { + stamp_ = stamp; + } /** * @brief Read-only access to the set of timestamps involved in this transaction * * @return An iterator range containing all involved timestamps, ordered oldest to newest */ - const_stamp_range involvedStamps() const {return involved_stamps_;} + const_stamp_range involvedStamps() const + { + return involved_stamps_; + } /** * @brief Read-only access to the minimum (oldest), timestamp among the transaction's stamp and @@ -131,7 +140,7 @@ class Transaction * * @return The minimum (oldest) timestamp. */ - const rclcpp::Time & minStamp() const; + const rclcpp::Time& minStamp() const; /** * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all @@ -139,7 +148,7 @@ class Transaction * * @return The maximum (newest) timestamp. */ - const rclcpp::Time & maxStamp() const; + const rclcpp::Time& maxStamp() const; /** * @brief Read-only access to the added constraints @@ -153,7 +162,10 @@ class Transaction * * @return An iterator range containing all removed constraint UUIDs */ - const_uuid_range removedConstraints() const {return removed_constraints_;} + const_uuid_range removedConstraints() const + { + return removed_constraints_; + } /** * @brief Read-only access to the added variables @@ -167,7 +179,10 @@ class Transaction * * @return An iterator range containing all removed variable UUIDs */ - const_uuid_range removedVariables() const {return removed_variables_;} + const_uuid_range removedVariables() const + { + return removed_variables_; + } /** * @brief Check if the transaction is empty, i.e. it has no added or removed constraints or @@ -185,7 +200,7 @@ class Transaction * * @param[in] stamp The timestamp to be added */ - void addInvolvedStamp(const rclcpp::Time & stamp); + void addInvolvedStamp(const rclcpp::Time& stamp); /** * @brief Add a constraint to this transaction @@ -210,7 +225,7 @@ class Transaction * * @param[in] constraint_uuid The UUID of the constraint to remove */ - void removeConstraint(const UUID & constraint_uuid); + void removeConstraint(const UUID& constraint_uuid); /** * @brief Add a variable to this transaction @@ -235,7 +250,7 @@ class Transaction * * @param[in] variable_uuid The UUID of the variable to remove */ - void removeVariable(const UUID & variable_uuid); + void removeVariable(const UUID& variable_uuid); /** * @brief Merge the contents of another transaction into this one. @@ -244,14 +259,14 @@ class Transaction * @param[in] overwrite Flag indicating that variables and constraints in \p other should * overwrite existing variables and constraints with the UUIDs. */ - void merge(const Transaction & other, bool overwrite = false); + void merge(const Transaction& other, bool overwrite = false); /** * @brief Print a human-readable description of the transaction to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; /** * @brief Perform a deep copy of the Transaction and return a unique pointer to the copy @@ -267,36 +282,36 @@ class Transaction * * @param[out] archive - The archive to serialize this constraint into */ - void serialize(fuse_core::BinaryOutputArchive & /* archive */) const; + void serialize(fuse_core::BinaryOutputArchive& /* archive */) const; /** * @brief Serialize this Constraint into the provided text archive * * @param[out] archive - The archive to serialize this constraint into */ - void serialize(fuse_core::TextOutputArchive & /* archive */) const; + void serialize(fuse_core::TextOutputArchive& /* archive */) const; /** * @brief Deserialize data from the provided binary archive into this Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - void deserialize(fuse_core::BinaryInputArchive & /* archive */); + void deserialize(fuse_core::BinaryInputArchive& /* archive */); /** * @brief Deserialize data from the provided text archive into this Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - void deserialize(fuse_core::TextInputArchive & /* archive */); + void deserialize(fuse_core::TextInputArchive& /* archive */); private: - rclcpp::Time stamp_{0, 0, RCL_ROS_TIME}; //!< The transaction message timestamp + rclcpp::Time stamp_{ 0, 0, RCL_ROS_TIME }; //!< The transaction message timestamp std::vector added_constraints_; //!< The constraints to be added - std::vector added_variables_; //!< The variables to be added - std::set involved_stamps_; //!< The set of timestamps involved in this transaction - std::vector removed_constraints_; //!< The constraint UUIDs to be removed - std::vector removed_variables_; //!< The variable UUIDs to be removed + std::vector added_variables_; //!< The variables to be added + std::set involved_stamps_; //!< The set of timestamps involved in this transaction + std::vector removed_constraints_; //!< The constraint UUIDs to be removed + std::vector removed_variables_; //!< The variable UUIDs to be removed // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -308,22 +323,22 @@ class Transaction * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & stamp_; - archive & added_constraints_; - archive & added_variables_; - archive & involved_stamps_; - archive & removed_constraints_; - archive & removed_variables_; + archive& stamp_; + archive& added_constraints_; + archive& added_variables_; + archive& involved_stamps_; + archive& removed_constraints_; + archive& removed_variables_; } }; /** * Stream operator for printing Transaction objects. */ -std::ostream & operator<<(std::ostream & stream, const Transaction & transaction); +std::ostream& operator<<(std::ostream& stream, const Transaction& transaction); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/transaction_deserializer.hpp b/fuse_core/include/fuse_core/transaction_deserializer.hpp index 634129b21..7d5f44675 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.hpp +++ b/fuse_core/include/fuse_core/transaction_deserializer.hpp @@ -46,9 +46,7 @@ namespace fuse_core /** * @brief Serialize a transaction into a message */ -void serializeTransaction( - const fuse_core::Transaction & transaction, - fuse_msgs::msg::SerializedTransaction & msg); +void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg); /** * @brief Deserialize a Transaction @@ -75,8 +73,8 @@ class TransactionDeserializer * @param[IN] msg The SerializedTransaction message to be deserialized * @return A fuse Transaction object */ - inline fuse_core::Transaction::UniquePtr deserialize( - const fuse_msgs::msg::SerializedTransaction::ConstSharedPtr msg) const + inline fuse_core::Transaction::UniquePtr + deserialize(const fuse_msgs::msg::SerializedTransaction::ConstSharedPtr msg) const { return deserialize(*msg); } @@ -90,8 +88,7 @@ class TransactionDeserializer * @param[IN] msg The SerializedTransaction message to be deserialized * @return A fuse Transaction object */ - fuse_core::Transaction::UniquePtr deserialize( - const fuse_msgs::msg::SerializedTransaction & msg) const; + fuse_core::Transaction::UniquePtr deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b3bdc4a55..444f7e574 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -54,15 +54,18 @@ namespace fuse_core * @param[in] z The quaternion x-axis component * @return The quaternion's Euler pitch angle component */ -template +template static inline T getPitch(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles const T sin_pitch = T(2.0) * (w * y - z * x); - if (ceres::abs(sin_pitch) >= T(1.0)) { + if (ceres::abs(sin_pitch) >= T(1.0)) + { return (sin_pitch >= T(0.0) ? T(1.0) : T(-1.0)) * T(M_PI / 2.0); - } else { + } + else + { return ceres::asin(sin_pitch); } } @@ -76,7 +79,7 @@ static inline T getPitch(const T w, const T x, const T y, const T z) * @param[in] z The quaternion x-axis component * @return The quaternion's Euler roll angle component */ -template +template static inline T getRoll(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -94,7 +97,7 @@ static inline T getRoll(const T w, const T x, const T y, const T z) * @param[in] z The quaternion x-axis component * @return The quaternion's Euler yaw angle component */ -template +template static inline T getYaw(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -109,8 +112,8 @@ static inline T getYaw(const T w, const T x, const T y, const T z) * @param[in/out] angle Input angle to be wrapped to the [-Pi, +Pi) range. Angle is updated by this * function. */ -template -void wrapAngle2D(T & angle) +template +void wrapAngle2D(T& angle) { // Define some necessary variations of PI with the correct type (double or Jet) static const T PI = T(M_PI); @@ -126,8 +129,8 @@ void wrapAngle2D(T & angle) * @param[in] angle Input angle to be wrapped to the (-Pi, +Pi] range. * @return The equivalent wrapped angle */ -template -T wrapAngle2D(const T & angle) +template +T wrapAngle2D(const T& angle) { T wrapped = angle; wrapAngle2D(wrapped); @@ -140,7 +143,7 @@ T wrapAngle2D(const T & angle) * @param[in] angle The rotation angle, in radians * @return The equivalent 2x2 rotation matrix */ -template +template Eigen::Matrix rotationMatrix2D(const T angle) { const T cos_angle = ceres::cos(angle); @@ -157,22 +160,24 @@ Eigen::Matrix rotationMatrix2D(const T angle) * @param[in] b The second element * @return The joined topic name */ -inline -std::string joinTopicName(std::string a, std::string b) +inline std::string joinTopicName(std::string a, std::string b) { - if (a.empty()) { + if (a.empty()) + { return b; } - if (b.empty()) { + if (b.empty()) + { return a; } - if (b.front() == '/' || b.front() == '~') { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), "Second argument to joinTopicName is absolute! Returning it."); + if (b.front() == '/' || b.front() == '~') + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), "Second argument to joinTopicName is absolute! Returning it."); return b; } - if (a.back() == '/') { + if (a.back() == '/') + { a.pop_back(); } diff --git a/fuse_core/include/fuse_core/uuid.hpp b/fuse_core/include/fuse_core/uuid.hpp index 35aa98575..fd9833577 100644 --- a/fuse_core/include/fuse_core/uuid.hpp +++ b/fuse_core/include/fuse_core/uuid.hpp @@ -52,131 +52,131 @@ namespace uuid { using boost::uuids::to_string; using hash = boost::hash; -constexpr UUID NIL = {{0}}; +constexpr UUID NIL = { { 0 } }; /** - * @brief Convert a string representation of the UUID into a UUID variable - * - * There are a few supported formats: - * - "01234567-89AB-CDEF-0123-456789ABCDEF" - * - "01234567-89ab-cdef-0123-456789abcdef" - * - "0123456789abcdef0123456789abcdef" - * - "{01234567-89ab-cdef-0123-456789abcdef}" - */ -inline UUID from_string(const std::string & uuid_string) + * @brief Convert a string representation of the UUID into a UUID variable + * + * There are a few supported formats: + * - "01234567-89AB-CDEF-0123-456789ABCDEF" + * - "01234567-89ab-cdef-0123-456789abcdef" + * - "0123456789abcdef0123456789abcdef" + * - "{01234567-89ab-cdef-0123-456789abcdef}" + */ +inline UUID from_string(const std::string& uuid_string) { return boost::uuids::string_generator()(uuid_string); } /** - * @brief Generate a random UUID - */ + * @brief Generate a random UUID + */ UUID generate(); /** - * @brief Generate a UUID from a raw data buffer - * - * @param[in] data A data buffer containing information that makes this item unique - * @param[in] byte_count The number of bytes in the data buffer - * @return A repeatable UUID specific to the provided data - */ -inline UUID generate(const void * data, size_t byte_count) + * @brief Generate a UUID from a raw data buffer + * + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided data + */ +inline UUID generate(const void* data, size_t byte_count) { return boost::uuids::name_generator(NIL)(data, byte_count); } /** - * @brief Generate a UUID from a C-style string - * - * @param[in] data A data buffer held in a C-style string - * @return A repeatable UUID specific to the provided data - */ -inline UUID generate(const char * data) + * @brief Generate a UUID from a C-style string + * + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided data + */ +inline UUID generate(const char* data) { return generate(data, std::strlen(data)); } /** - * @brief Generate a UUID from a C++ string - * - * @param[in] data A data buffer held in a C++-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & data) + * @brief Generate a UUID from a C++ string + * + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& data) { return generate(data.c_str(), data.length()); } /** - * @brief Generate a UUID from a namespace string and a raw data buffer - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer containing information that makes this item unique - * @param[in] byte_count The number of bytes in the data buffer - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const void * data, size_t byte_count) + * @brief Generate a UUID from a namespace string and a raw data buffer + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const void* data, size_t byte_count) { return boost::uuids::name_generator(generate(namespace_string))(data, byte_count); } /** - * @brief Generate a UUID from a namespace string and C-style string - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer held in a C-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const char * data) + * @brief Generate a UUID from a namespace string and C-style string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const char* data) { return generate(namespace_string, data, std::strlen(data)); } /** - * @brief Generate a UUID from a namespace string and a C++ string - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer held in a C++-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const std::string & data) + * @brief Generate a UUID from a namespace string and a C++ string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const std::string& data) { return generate(namespace_string, data.c_str(), data.length()); } /** - * @brief Generate a UUID from a namespace string and a ros timestamp - * - * Every unique timestamp will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] stamp An rclcpp::Time timestamp - * @return A repeatable UUID specific to the provided namespace and timestamp - */ -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp); + * @brief Generate a UUID from a namespace string and a ros timestamp + * + * Every unique timestamp will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp An rclcpp::Time timestamp + * @return A repeatable UUID specific to the provided namespace and timestamp + */ +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp); /** - * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id - * - * Every unique timestamp and id pair will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] stamp A rclcpp::Time timestamp - * @param[in] id A UUID - * @return A repeatable UUID specific to the provided namespace and timestamp - */ -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, const UUID & id); + * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id + * + * Every unique timestamp and id pair will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp A rclcpp::Time timestamp + * @param[in] id A UUID + * @return A repeatable UUID specific to the provided namespace and timestamp + */ +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id); /** - * @brief Generate a UUID from a namespace string and a user provided id - * - * Every unique user id will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] user_id A uint64_t user generated id - * @return A repeatable UUID specific to the provided namespace and user id - */ -UUID generate(const std::string & namespace_string, const uint64_t & user_id); + * @brief Generate a UUID from a namespace string and a user provided id + * + * Every unique user id will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] user_id A uint64_t user generated id + * @return A repeatable UUID specific to the provided namespace and user id + */ +UUID generate(const std::string& namespace_string, const uint64_t& user_id); } // namespace uuid } // namespace fuse_core @@ -188,10 +188,10 @@ namespace std * @brief Define a hash specialization for the UUID to make it easier to use in unordered_maps and * unordered_sets */ -template<> +template <> struct hash { - size_t operator()(const fuse_core::UUID & id) const + size_t operator()(const fuse_core::UUID& id) const { return boost::uuids::hash_value(id); } diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 91570d966..550021977 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -61,10 +61,10 @@ * } * @endcode */ -#define FUSE_VARIABLE_CLONE_DEFINITION(...) \ - fuse_core::Variable::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_VARIABLE_CLONE_DEFINITION(...) \ + fuse_core::Variable::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -80,22 +80,22 @@ * } * @endcode */ -#define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#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; \ } /** @@ -113,17 +113,17 @@ * } * @endcode */ -#define FUSE_VARIABLE_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#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(); \ } /** @@ -140,10 +140,10 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_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__) /** @@ -160,13 +160,12 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -219,7 +218,7 @@ class Variable * * @param[in] uuid The unique ID number for this variable */ - explicit Variable(const UUID & uuid); + explicit Variable(const UUID& uuid); /** * @brief Destructor @@ -229,7 +228,10 @@ class Variable /** * @brief Returns a UUID for this variable. */ - const UUID & uuid() const {return uuid_;} + const UUID& uuid() const + { + return uuid_; + } /** * @brief Returns a unique name for this variable type. @@ -264,7 +266,10 @@ class Variable * 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();} + virtual size_t localSize() const + { + return size(); + } /** * @brief Read-only access to the variable data @@ -274,7 +279,7 @@ class Variable * 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; + virtual const double* data() const = 0; /** * @brief Read-write access to the variable data @@ -284,14 +289,14 @@ class Variable * Variable::size() elements will be accessed externally. This interface is provided for * integration with Ceres, which uses raw pointers. */ - virtual double * data() = 0; + virtual double* data() = 0; /** * @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; + 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 @@ -326,7 +331,7 @@ class Variable * * @return A base pointer to an instance of a derived LocalParameterization */ - virtual fuse_core::LocalParameterization * localParameterization() const + virtual fuse_core::LocalParameterization* localParameterization() const { return nullptr; } @@ -349,16 +354,19 @@ class Variable * * @return A base pointer to an instance of a derived Manifold */ - virtual fuse_core::Manifold * manifold() const + 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) { + if (!local_parameterization) + { return nullptr; - } else { + } + else + { return new fuse_core::ManifoldAdapter(local_parameterization); } } @@ -408,7 +416,7 @@ class Variable * * @param[out] archive - The archive to serialize this variable into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Variable into the provided text archive @@ -420,7 +428,7 @@ class Variable * * @param[out] archive - The archive to serialize this variable into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Variable @@ -432,7 +440,7 @@ class Variable * * @param[in] archive - The archive holding serialized Variable data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Variable @@ -444,7 +452,7 @@ class Variable * * @param[in] archive - The archive holding serialized Variable data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: fuse_core::UUID uuid_; //!< The unique ID number for this variable @@ -464,17 +472,17 @@ class Variable * @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 */) + 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/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index c2be0ad85..b3fd5da11 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -46,9 +46,7 @@ namespace fuse_core { -AsyncMotionModel::AsyncMotionModel(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncMotionModel::AsyncMotionModel(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -57,7 +55,7 @@ AsyncMotionModel::~AsyncMotionModel() internal_stop(); } -bool AsyncMotionModel::apply(Transaction & transaction) +bool AsyncMotionModel::apply(Transaction& transaction) { // Insert a call to the motion model's queryCallback() function into the motion model's callback // queue. While this makes this particular function more difficult to write, it does simplify the @@ -66,8 +64,8 @@ bool AsyncMotionModel::apply(Transaction & transaction) // service callback, and should be a familiar pattern for ROS developers. This function blocks // until the queryCallback() call completes, thus enforcing that motion models are generated in // order. - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::applyCallback, this, std::ref(transaction))); + auto callback = + std::make_shared>(std::bind(&AsyncMotionModel::applyCallback, this, std::ref(transaction))); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -75,11 +73,11 @@ bool AsyncMotionModel::apply(Transaction & transaction) return result.get(); } -void AsyncMotionModel::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void AsyncMotionModel::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncMotionModel!"); } @@ -91,18 +89,19 @@ void AsyncMotionModel::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization @@ -114,19 +113,13 @@ void AsyncMotionModel::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -134,17 +127,14 @@ void AsyncMotionModel::initialize( void AsyncMotionModel::graphCallback(Graph::ConstSharedPtr graph) { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph)) - ); + auto callback = + std::make_shared>(std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph))); callback_queue_->addCallback(callback); } void AsyncMotionModel::start() { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onStart, this) - ); + auto callback = std::make_shared>(std::bind(&AsyncMotionModel::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -154,14 +144,15 @@ void AsyncMotionModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncMotionModel::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. @@ -172,7 +163,8 @@ void AsyncMotionModel::stop() void AsyncMotionModel::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index 3caeadb53..cf174247a 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -37,9 +37,7 @@ namespace fuse_core { -AsyncPublisher::AsyncPublisher(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncPublisher::AsyncPublisher(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -48,11 +46,11 @@ AsyncPublisher::~AsyncPublisher() internal_stop(); } -void AsyncPublisher::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void AsyncPublisher::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncPublisher!"); } @@ -64,18 +62,19 @@ void AsyncPublisher::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization @@ -87,19 +86,13 @@ void AsyncPublisher::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -110,14 +103,13 @@ void AsyncPublisher::notify(Transaction::ConstSharedPtr transaction, Graph::Cons // Insert a call to the `notifyCallback` method into the internal callback queue. // This minimizes the time spent by the optimizer's thread calling this function. auto callback = std::make_shared>( - std::bind(&AsyncPublisher::notifyCallback, this, std::move(transaction), std::move(graph))); + std::bind(&AsyncPublisher::notifyCallback, this, std::move(transaction), std::move(graph))); callback_queue_->addCallback(callback); } void AsyncPublisher::start() { - auto callback = - std::make_shared>(std::bind(&AsyncPublisher::onStart, this)); + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -127,14 +119,15 @@ void AsyncPublisher::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncPublisher::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. @@ -145,7 +138,8 @@ void AsyncPublisher::stop() void AsyncPublisher::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 80f926448..9f9903737 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -45,9 +45,7 @@ namespace fuse_core { -AsyncSensorModel::AsyncSensorModel(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncSensorModel::AsyncSensorModel(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -56,12 +54,11 @@ AsyncSensorModel::~AsyncSensorModel() internal_stop(); } -void AsyncSensorModel::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) +void AsyncSensorModel::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name, TransactionCallback transaction_callback) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncSensorModel!"); } @@ -73,18 +70,19 @@ void AsyncSensorModel::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); transaction_callback_ = transaction_callback; @@ -98,19 +96,13 @@ void AsyncSensorModel::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -118,9 +110,8 @@ void AsyncSensorModel::initialize( void AsyncSensorModel::graphCallback(Graph::ConstSharedPtr graph) { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph)) - ); + auto callback = + std::make_shared>(std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph))); callback_queue_->addCallback(callback); } @@ -131,9 +122,7 @@ void AsyncSensorModel::sendTransaction(Transaction::SharedPtr transaction) void AsyncSensorModel::start() { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onStart, this) - ); + auto callback = std::make_shared>(std::bind(&AsyncSensorModel::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -143,14 +132,15 @@ void AsyncSensorModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncSensorModel::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. @@ -161,7 +151,8 @@ void AsyncSensorModel::stop() void AsyncSensorModel::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index 454cf6302..84dc2fba3 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -39,29 +39,30 @@ namespace fuse_core CallbackAdapter::CallbackAdapter(std::shared_ptr context_ptr) { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); // Guard condition is used by the wait set to handle execute-or-not logic gc_ = rcl_get_zero_initialized_guard_condition(); - if (RCL_RET_OK != rcl_guard_condition_init( - &gc_, context_ptr->get_rcl_context().get(), guard_condition_options)) + if (RCL_RET_OK != rcl_guard_condition_init(&gc_, context_ptr->get_rcl_context().get(), guard_condition_options)) { throw std::runtime_error("Could not init guard condition for callback waitable."); } } /** - * @brief tell the CallbackGroup how many guard conditions are ready in this waitable - */ -size_t CallbackAdapter::get_number_of_ready_guard_conditions() {return 1;} + * @brief tell the CallbackGroup how many guard conditions are ready in this waitable + */ +size_t CallbackAdapter::get_number_of_ready_guard_conditions() +{ + return 1; +} /** - * @brief tell the CallbackGroup that this waitable is ready to execute anything - */ -bool CallbackAdapter::is_ready(rcl_wait_set_t const & wait_set) + * @brief tell the CallbackGroup that this waitable is ready to execute anything + */ +bool CallbackAdapter::is_ready(rcl_wait_set_t const& wait_set) { - (void) wait_set; + (void)wait_set; return !callback_queue_.empty(); } @@ -71,32 +72,35 @@ bool CallbackAdapter::is_ready(rcl_wait_set_t const & wait_set) waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ -void CallbackAdapter::add_to_wait_set(rcl_wait_set_t & wait_set) +void CallbackAdapter::add_to_wait_set(rcl_wait_set_t& wait_set) { - if (RCL_RET_OK != rcl_wait_set_add_guard_condition(&wait_set, &gc_, NULL)) { + if (RCL_RET_OK != rcl_wait_set_add_guard_condition(&wait_set, &gc_, NULL)) + { RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not add callback waitable to wait set."); } } /** - * @brief check the callback queue and return the next callback to run - * - */ + * @brief check the callback queue and return the next callback to run + * + */ std::shared_ptr CallbackAdapter::take_data() { std::shared_ptr cb_wrapper = nullptr; // fetch the callback ptr and release the lock without spending time in the callback { std::lock_guard lock(queue_mutex_); - if (!callback_queue_.empty()) { + if (!callback_queue_.empty()) + { cb_wrapper = callback_queue_.front(); callback_queue_.pop_front(); } - if (!callback_queue_.empty()) { + if (!callback_queue_.empty()) + { // Trigger so executor wakes again - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), "Could not trigger guard condition for callback"); + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not trigger guard condition for callback"); } } } @@ -104,38 +108,39 @@ std::shared_ptr CallbackAdapter::take_data() } /** - * @brief hook that allows the rclcpp::waitables interface to run the next callback - * - */ -void CallbackAdapter::execute(std::shared_ptr const & data) + * @brief hook that allows the rclcpp::waitables interface to run the next callback + * + */ +void CallbackAdapter::execute(std::shared_ptr const& data) { - if (!data) { + if (!data) + { throw std::runtime_error("'data' is empty"); } std::static_pointer_cast(data)->call(); } -void CallbackAdapter::addCallback(const std::shared_ptr & callback) +void CallbackAdapter::addCallback(const std::shared_ptr& callback) { std::lock_guard lock(queue_mutex_); callback_queue_.push_back(callback); - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), - "Could not trigger guard condition for callback, pulling callback off the queue."); - callback_queue_.pop_back(); // Undo + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo } } -void CallbackAdapter::addCallback(std::shared_ptr && callback) +void CallbackAdapter::addCallback(std::shared_ptr&& callback) { std::lock_guard lock(queue_mutex_); callback_queue_.push_back(std::move(callback)); - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), - "Could not trigger guard condition for callback, pulling callback off the queue."); - callback_queue_.pop_back(); // Undo + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo } } @@ -145,5 +150,4 @@ void CallbackAdapter::removeAllCallbacks() callback_queue_.clear(); } - } // namespace fuse_core diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 1919e7f82..960fd3c00 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -54,699 +54,471 @@ namespace fuse_core { void loadCovarianceOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Covariance::Options & covariance_options, - const std::string & ns) + node_interfaces::NodeInterfaces + interfaces, + ceres::Covariance::Options& covariance_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; // The sparse_linear_algebra_library_type field was added to ceres::Covariance::Options in version // 1.13.0, see https://github.com/ceres-solver/ceres- // solver/commit/14d8297cf968e421c5db4e3fb0543b3b111155d7 - covariance_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), - covariance_options.sparse_linear_algebra_library_type); - covariance_options.algorithm_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "algorithm_type"), - covariance_options.algorithm_type); - - tmp_descr.description = ( - "If DENSE_SVD is used, this parameter sets the threshold for determining if a Jacobian matrix " - "is rank deficient following the condition: " - "\n" - "min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)" - "\n" - "Where min_sigma and max_sigma are the minimum and maximum singular values of J respectively."); - covariance_options.min_reciprocal_condition_number = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_reciprocal_condition_number"), - covariance_options.min_reciprocal_condition_number, - tmp_descr - ); + covariance_options.sparse_linear_algebra_library_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), + covariance_options.sparse_linear_algebra_library_type); + covariance_options.algorithm_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "algorithm_type"), covariance_options.algorithm_type); tmp_descr.description = - "The number of singular dimensions to tolerate (-1 unbounded) no effect on `SPARSE_QR`"; + ("If DENSE_SVD is used, this parameter sets the threshold for determining if a Jacobian matrix " + "is rank deficient following the condition: " + "\n" + "min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)" + "\n" + "Where min_sigma and max_sigma are the minimum and maximum singular values of J respectively."); + covariance_options.min_reciprocal_condition_number = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_reciprocal_condition_number"), + covariance_options.min_reciprocal_condition_number, tmp_descr); + + tmp_descr.description = "The number of singular dimensions to tolerate (-1 unbounded) no effect on `SPARSE_QR`"; covariance_options.null_space_rank = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "null_space_rank"), - covariance_options.null_space_rank, tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "null_space_rank"), covariance_options.null_space_rank, tmp_descr); + + tmp_descr.description = "Number of threads to be used for evaluating the Jacobian and estimation of covariance"; + covariance_options.num_threads = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "num_threads"), + covariance_options.num_threads, tmp_descr); tmp_descr.description = - "Number of threads to be used for evaluating the Jacobian and estimation of covariance"; - covariance_options.num_threads = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "num_threads"), covariance_options.num_threads, tmp_descr - ); - - tmp_descr.description = ( - "false will turn off the application of the loss function to the output of the cost function " - "and in turn its effect on the covariance (does not affect residual blocks with built-in loss " - "functions)"); - covariance_options.apply_loss_function = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "apply_loss_function"), - covariance_options.apply_loss_function, - tmp_descr - ); + ("false will turn off the application of the loss function to the output of the cost function " + "and in turn its effect on the covariance (does not affect residual blocks with built-in loss " + "functions)"); + covariance_options.apply_loss_function = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "apply_loss_function"), + covariance_options.apply_loss_function, tmp_descr); } -void loadProblemOptionsFromROS( - node_interfaces::NodeInterfaces interfaces, - ceres::Problem::Options & problem_options, - const std::string & ns) +void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, + ceres::Problem::Options& problem_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; tmp_descr.description = "trades memory for faster Problem::RemoveResidualBlock()"; - problem_options.enable_fast_removal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "enable_fast_removal"), - problem_options.enable_fast_removal, - tmp_descr - ); - - tmp_descr.description = ( - "By default, Ceres performs a variety of safety checks when constructing " - "the problem. There is a small but measurable performance penalty to these " - "checks, typically around 5% of construction time. If you are sure your " - "problem construction is correct, and 5% of the problem construction time " - "is truly an overhead you want to avoid, then you can set " - "disable_all_safety_checks to true." - "\n" - "WARNING: Do not set this to true, unless you are absolutely sure of what " - "you are doing"); - problem_options.disable_all_safety_checks = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "disable_all_safety_checks"), - problem_options.disable_all_safety_checks, - tmp_descr - ); + problem_options.enable_fast_removal = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "enable_fast_removal"), + problem_options.enable_fast_removal, tmp_descr); + + tmp_descr.description = ("By default, Ceres performs a variety of safety checks when constructing " + "the problem. There is a small but measurable performance penalty to these " + "checks, typically around 5% of construction time. If you are sure your " + "problem construction is correct, and 5% of the problem construction time " + "is truly an overhead you want to avoid, then you can set " + "disable_all_safety_checks to true." + "\n" + "WARNING: Do not set this to true, unless you are absolutely sure of what " + "you are doing"); + problem_options.disable_all_safety_checks = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_all_safety_checks"), + problem_options.disable_all_safety_checks, tmp_descr); } void loadSolverOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Solver::Options & solver_options, - const std::string & ns) + node_interfaces::NodeInterfaces + interfaces, + ceres::Solver::Options& solver_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; // Minimizer options solver_options.minimizer_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "minimizer_type"), solver_options.minimizer_type); - solver_options.line_search_direction_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_direction_type"), - solver_options.line_search_direction_type - ); + interfaces, fuse_core::joinParameterName(ns, "minimizer_type"), solver_options.minimizer_type); + solver_options.line_search_direction_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "line_search_direction_type"), + solver_options.line_search_direction_type); solver_options.line_search_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_type"), - solver_options.line_search_type - ); - solver_options.nonlinear_conjugate_gradient_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "nonlinear_conjugate_gradient_type"), - solver_options.nonlinear_conjugate_gradient_type - ); - - tmp_descr.description = ( - "The rank of the LBFGS hessian approximation. See: Nocedal, J. (1980). 'Updating Quasi-Newton " - "Matrices with Limited Storage'. Mathematics of Computation 35 (151): 773-782."); - solver_options.max_lbfgs_rank = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_lbfgs_rank"), - solver_options.max_lbfgs_rank, - tmp_descr - ); - - tmp_descr.description = ( - "As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), " - "the initial inverse Hessian approximation is taken to be the Identity. " - "However, Oren showed that using instead I * \\gamma, where \\gamma is " - "chosen to approximate an eigenvalue of the true inverse Hessian can " - "result in improved convergence in a wide variety of cases. Setting " - "use_approximate_eigenvalue_bfgs_scaling to true enables this scaling."); - solver_options.use_approximate_eigenvalue_bfgs_scaling = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_approximate_eigenvalue_bfgs_scaling"), - solver_options.use_approximate_eigenvalue_bfgs_scaling, - tmp_descr - ); - - tmp_descr.description = ( - "Degree of the polynomial used to approximate the objective function. Valid values are " - "BISECTION, QUADRATIC and CUBIC."); - solver_options.line_search_interpolation_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_interpolation_type"), - solver_options.line_search_interpolation_type); + interfaces, fuse_core::joinParameterName(ns, "line_search_type"), solver_options.line_search_type); + solver_options.nonlinear_conjugate_gradient_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "nonlinear_conjugate_gradient_type"), + solver_options.nonlinear_conjugate_gradient_type); tmp_descr.description = - "If during the line search, the step_size falls below this value, it is truncated to zero."; - solver_options.min_line_search_step_size = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_line_search_step_size"), - solver_options.min_line_search_step_size, - tmp_descr - ); + ("The rank of the LBFGS hessian approximation. See: Nocedal, J. (1980). 'Updating Quasi-Newton " + "Matrices with Limited Storage'. Mathematics of Computation 35 (151): 773-782."); + solver_options.max_lbfgs_rank = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_lbfgs_rank"), + solver_options.max_lbfgs_rank, tmp_descr); + + tmp_descr.description = ("As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), " + "the initial inverse Hessian approximation is taken to be the Identity. " + "However, Oren showed that using instead I * \\gamma, where \\gamma is " + "chosen to approximate an eigenvalue of the true inverse Hessian can " + "result in improved convergence in a wide variety of cases. Setting " + "use_approximate_eigenvalue_bfgs_scaling to true enables this scaling."); + solver_options.use_approximate_eigenvalue_bfgs_scaling = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_approximate_eigenvalue_bfgs_scaling"), + solver_options.use_approximate_eigenvalue_bfgs_scaling, tmp_descr); + + tmp_descr.description = ("Degree of the polynomial used to approximate the objective function. Valid values are " + "BISECTION, QUADRATIC and CUBIC."); + solver_options.line_search_interpolation_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "line_search_interpolation_type"), + solver_options.line_search_interpolation_type); + + tmp_descr.description = "If during the line search, the step_size falls below this value, it is truncated to zero."; + solver_options.min_line_search_step_size = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_line_search_step_size"), + solver_options.min_line_search_step_size, tmp_descr); // Line search parameters - tmp_descr.description = ( - "Solving the line search problem exactly is computationally " - "prohibitive. Fortunately, line search based optimization " - "algorithms can still guarantee convergence if instead of an " - "exact solution, the line search algorithm returns a solution " - "which decreases the value of the objective function " - "sufficiently. More precisely, we are looking for a step_size: " - "s.t. " - "f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size"); - solver_options.line_search_sufficient_function_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_sufficient_function_decrease"), - solver_options.line_search_sufficient_function_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "In each iteration of the line search, " - "new_step_size >= max_line_search_step_contraction * step_size" - "\n" - "Note that by definition, for contraction: " - "0 < max_step_contraction < min_step_contraction < 1"); - solver_options.max_line_search_step_contraction = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_line_search_step_contraction"), - solver_options.max_line_search_step_contraction, - tmp_descr - ); - - tmp_descr.description = ( - "In each iteration of the line search, " - "new_step_size <= min_line_search_step_contraction * step_size" - "\n" - "Note that by definition, for contraction: " - "0 < max_step_contraction < min_step_contraction < 1"); - solver_options.min_line_search_step_contraction = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_line_search_step_contraction"), - solver_options.min_line_search_step_contraction, - tmp_descr - ); - - tmp_descr.description = ( - "Maximum number of trial step size iterations during each line " - "search, if a step size satisfying the search conditions cannot " - "be found within this number of trials, the line search will " - "terminate. " - - "The minimum allowed value is 0 for trust region minimizer and 1 " - "otherwise. If 0 is specified for the trust region minimizer, " - "then line search will not be used when solving constrained " - "optimization problems. "); - solver_options.max_num_line_search_step_size_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_line_search_step_size_iterations"), - solver_options.max_num_line_search_step_size_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Maximum number of restarts of the line search direction algorithm before " - "terminating the optimization. Restarts of the line search direction " - "algorithm occur when the current algorithm fails to produce a new descent " - "direction. This typically indicates a numerical failure, or a breakdown " - "in the validity of the approximations used. "); - solver_options.max_num_line_search_direction_restarts = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_line_search_direction_restarts"), - solver_options.max_num_line_search_direction_restarts, - tmp_descr - ); - - tmp_descr.description = ( - "The strong Wolfe conditions consist of the Armijo sufficient " - "decrease condition, and an additional requirement that the " - "step-size be chosen s.t. the _magnitude_ ('strong' Wolfe " - "conditions) of the gradient along the search direction " - "decreases sufficiently. Precisely, this second condition " - "is that we seek a step_size s.t. " - "\n" - " |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|" - "\n" - "Where f() is the line search objective and f'() is the derivative of f " - "w.r.t step_size (d f / d step_size)."); - solver_options.line_search_sufficient_curvature_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_sufficient_curvature_decrease"), - solver_options.line_search_sufficient_curvature_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "During the bracketing phase of the Wolfe search, the step size is " - "increased until either a point satisfying the Wolfe conditions is " - "found, or an upper bound for a bracket containing a point satisfying " - "the conditions is found. Precisely, at each iteration of the expansion:" - "\n" - " new_step_size <= max_step_expansion * step_size." - "\n" - "By definition for expansion, max_step_expansion > 1.0."); - solver_options.max_line_search_step_expansion = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_line_search_step_expansion"), - solver_options.max_line_search_step_expansion, - tmp_descr - ); - - solver_options.trust_region_strategy_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_strategy_type"), - solver_options.trust_region_strategy_type - ); - solver_options.dogleg_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "dogleg_type"), solver_options.dogleg_type); - - tmp_descr.description = ( - "Enables the non-monotonic trust region algorithm as described by Conn, " - "Gould & Toint in 'Trust Region Methods', Section 10.1"); - solver_options.use_nonmonotonic_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_nonmonotonic_steps"), - solver_options.use_nonmonotonic_steps, - tmp_descr - ); - - tmp_descr.description = - "The window size used by the step selection algorithm to accept non-monotonic steps"; - solver_options.max_consecutive_nonmonotonic_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_consecutive_nonmonotonic_steps"), - solver_options.max_consecutive_nonmonotonic_steps, - tmp_descr - ); + tmp_descr.description = ("Solving the line search problem exactly is computationally " + "prohibitive. Fortunately, line search based optimization " + "algorithms can still guarantee convergence if instead of an " + "exact solution, the line search algorithm returns a solution " + "which decreases the value of the objective function " + "sufficiently. More precisely, we are looking for a step_size: " + "s.t. " + "f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size"); + solver_options.line_search_sufficient_function_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "line_search_sufficient_function_decrease"), + solver_options.line_search_sufficient_function_decrease, tmp_descr); + + tmp_descr.description = ("In each iteration of the line search, " + "new_step_size >= max_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.max_line_search_step_contraction = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_line_search_step_contraction"), + solver_options.max_line_search_step_contraction, tmp_descr); + + tmp_descr.description = ("In each iteration of the line search, " + "new_step_size <= min_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.min_line_search_step_contraction = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_line_search_step_contraction"), + solver_options.min_line_search_step_contraction, tmp_descr); + + tmp_descr.description = ("Maximum number of trial step size iterations during each line " + "search, if a step size satisfying the search conditions cannot " + "be found within this number of trials, the line search will " + "terminate. " + + "The minimum allowed value is 0 for trust region minimizer and 1 " + "otherwise. If 0 is specified for the trust region minimizer, " + "then line search will not be used when solving constrained " + "optimization problems. "); + solver_options.max_num_line_search_step_size_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_line_search_step_size_iterations"), + solver_options.max_num_line_search_step_size_iterations, tmp_descr); + + tmp_descr.description = ("Maximum number of restarts of the line search direction algorithm before " + "terminating the optimization. Restarts of the line search direction " + "algorithm occur when the current algorithm fails to produce a new descent " + "direction. This typically indicates a numerical failure, or a breakdown " + "in the validity of the approximations used. "); + solver_options.max_num_line_search_direction_restarts = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_line_search_direction_restarts"), + solver_options.max_num_line_search_direction_restarts, tmp_descr); + + tmp_descr.description = ("The strong Wolfe conditions consist of the Armijo sufficient " + "decrease condition, and an additional requirement that the " + "step-size be chosen s.t. the _magnitude_ ('strong' Wolfe " + "conditions) of the gradient along the search direction " + "decreases sufficiently. Precisely, this second condition " + "is that we seek a step_size s.t. " + "\n" + " |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|" + "\n" + "Where f() is the line search objective and f'() is the derivative of f " + "w.r.t step_size (d f / d step_size)."); + solver_options.line_search_sufficient_curvature_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "line_search_sufficient_curvature_decrease"), + solver_options.line_search_sufficient_curvature_decrease, tmp_descr); + + tmp_descr.description = ("During the bracketing phase of the Wolfe search, the step size is " + "increased until either a point satisfying the Wolfe conditions is " + "found, or an upper bound for a bracket containing a point satisfying " + "the conditions is found. Precisely, at each iteration of the expansion:" + "\n" + " new_step_size <= max_step_expansion * step_size." + "\n" + "By definition for expansion, max_step_expansion > 1.0."); + solver_options.max_line_search_step_expansion = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_line_search_step_expansion"), + solver_options.max_line_search_step_expansion, tmp_descr); + + solver_options.trust_region_strategy_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "trust_region_strategy_type"), + solver_options.trust_region_strategy_type); + solver_options.dogleg_type = fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "dogleg_type"), + solver_options.dogleg_type); + + tmp_descr.description = ("Enables the non-monotonic trust region algorithm as described by Conn, " + "Gould & Toint in 'Trust Region Methods', Section 10.1"); + solver_options.use_nonmonotonic_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_nonmonotonic_steps"), + solver_options.use_nonmonotonic_steps, tmp_descr); + + tmp_descr.description = "The window size used by the step selection algorithm to accept non-monotonic steps"; + solver_options.max_consecutive_nonmonotonic_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_consecutive_nonmonotonic_steps"), + solver_options.max_consecutive_nonmonotonic_steps, tmp_descr); tmp_descr.description = "Maximum number of iterations for which the solver should run"; solver_options.max_num_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_iterations"), - solver_options.max_num_iterations, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "max_num_iterations"), solver_options.max_num_iterations, tmp_descr); tmp_descr.description = "Maximum amount of time for which the solver should run"; - solver_options.max_solver_time_in_seconds = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_solver_time_in_seconds"), - solver_options.max_solver_time_in_seconds, - tmp_descr - ); + solver_options.max_solver_time_in_seconds = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_solver_time_in_seconds"), + solver_options.max_solver_time_in_seconds, tmp_descr); tmp_descr.description = "Number of threads used by Ceres for evaluating the cost and jacobians"; - solver_options.num_threads = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "num_threads"), - solver_options.num_threads, - tmp_descr - ); - - tmp_descr.description = ( - "The size of the initial trust region. When the LEVENBERG_MARQUARDT strategy is used, the " - "reciprocal of this number is the initial regularization parameter"); - solver_options.initial_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "initial_trust_region_radius"), - solver_options.initial_trust_region_radius, - tmp_descr - ); + solver_options.num_threads = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "num_threads"), + solver_options.num_threads, tmp_descr); + + tmp_descr.description = ("The size of the initial trust region. When the LEVENBERG_MARQUARDT strategy is used, the " + "reciprocal of this number is the initial regularization parameter"); + solver_options.initial_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_trust_region_radius"), + solver_options.initial_trust_region_radius, tmp_descr); tmp_descr.description = "The trust region radius is not allowed to grow beyond this value"; - solver_options.max_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_trust_region_radius"), - solver_options.max_trust_region_radius, - tmp_descr - ); + solver_options.max_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_trust_region_radius"), + solver_options.max_trust_region_radius, tmp_descr); + + tmp_descr.description = "The solver terminates when the trust region becomes smaller than this value"; + solver_options.min_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_trust_region_radius"), + solver_options.min_trust_region_radius, tmp_descr); + + tmp_descr.description = "Lower threshold for relative decrease before a trust-region step is accepted"; + solver_options.min_relative_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_relative_decrease"), + solver_options.min_relative_decrease, tmp_descr); tmp_descr.description = - "The solver terminates when the trust region becomes smaller than this value"; - solver_options.min_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_trust_region_radius"), - solver_options.min_trust_region_radius, - tmp_descr - ); + ("The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the lower bound on the values of this diagonal matrix"); + solver_options.min_lm_diagonal = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_lm_diagonal"), + solver_options.min_lm_diagonal, tmp_descr); tmp_descr.description = - "Lower threshold for relative decrease before a trust-region step is accepted"; - solver_options.min_relative_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_relative_decrease"), - solver_options.min_relative_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " - "This is the lower bound on the values of this diagonal matrix"); - solver_options.min_lm_diagonal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_lm_diagonal"), - solver_options.min_lm_diagonal, - tmp_descr - ); - - tmp_descr.description = ( - "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " - "This is the upper bound on the values of this diagonal matrix"); - solver_options.max_lm_diagonal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_lm_diagonal"), - solver_options.max_lm_diagonal, - tmp_descr - ); - - tmp_descr.description = ( - "The step returned by a trust region strategy can sometimes be numerically invalid, usually " - "because of conditioning issues. Instead of crashing or stopping the optimization, the " - "optimizer can go ahead and try solving with a smaller trust region/better conditioned problem." - " This parameter sets the number of consecutive retries before the minimizer gives up"); - solver_options.max_num_consecutive_invalid_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_consecutive_invalid_steps"), - solver_options.max_num_consecutive_invalid_steps, - tmp_descr - ); + ("The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the upper bound on the values of this diagonal matrix"); + solver_options.max_lm_diagonal = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_lm_diagonal"), + solver_options.max_lm_diagonal, tmp_descr); tmp_descr.description = - "Minimizer terminates when: (new_cost - old_cost) < function_tolerance * old_cost;"; + ("The step returned by a trust region strategy can sometimes be numerically invalid, usually " + "because of conditioning issues. Instead of crashing or stopping the optimization, the " + "optimizer can go ahead and try solving with a smaller trust region/better conditioned problem." + " This parameter sets the number of consecutive retries before the minimizer gives up"); + solver_options.max_num_consecutive_invalid_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_consecutive_invalid_steps"), + solver_options.max_num_consecutive_invalid_steps, tmp_descr); + + tmp_descr.description = "Minimizer terminates when: (new_cost - old_cost) < function_tolerance * old_cost;"; solver_options.function_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "function_tolerance"), - solver_options.function_tolerance, - tmp_descr - ); - - tmp_descr.description = ( - "Minimizer terminates when: max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance" - "\n" - "This value should typically be 1e-4 * function_tolerance"); + interfaces, fuse_core::joinParameterName(ns, "function_tolerance"), solver_options.function_tolerance, tmp_descr); + + tmp_descr.description = ("Minimizer terminates when: max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance" + "\n" + "This value should typically be 1e-4 * function_tolerance"); solver_options.gradient_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_tolerance"), - solver_options.gradient_tolerance, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "gradient_tolerance"), solver_options.gradient_tolerance, tmp_descr); tmp_descr.description = - "Minimizer terminates when: |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)"; - solver_options.parameter_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "parameter_tolerance"), - solver_options.parameter_tolerance, - tmp_descr - ); - - solver_options.linear_solver_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "linear_solver_type"), - solver_options.linear_solver_type); - solver_options.preconditioner_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "preconditioner_type"), - solver_options.preconditioner_type); + "Minimizer terminates when: |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)"; + solver_options.parameter_tolerance = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "parameter_tolerance"), + solver_options.parameter_tolerance, tmp_descr); + + solver_options.linear_solver_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "linear_solver_type"), solver_options.linear_solver_type); + solver_options.preconditioner_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "preconditioner_type"), solver_options.preconditioner_type); solver_options.visibility_clustering_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "visibility_clustering_type"), - solver_options.visibility_clustering_type); + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "visibility_clustering_type"), + solver_options.visibility_clustering_type); solver_options.dense_linear_algebra_library_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "dense_linear_algebra_library_type"), - solver_options.dense_linear_algebra_library_type); - solver_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), - solver_options.sparse_linear_algebra_library_type); + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "dense_linear_algebra_library_type"), + solver_options.dense_linear_algebra_library_type); + solver_options.sparse_linear_algebra_library_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), + solver_options.sparse_linear_algebra_library_type); // No parameter is loaded for: std::shared_ptr linear_solver_ordering; - tmp_descr.description = - "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; - solver_options.use_explicit_schur_complement = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_explicit_schur_complement"), - solver_options.use_explicit_schur_complement, - tmp_descr - ); + tmp_descr.description = "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; + solver_options.use_explicit_schur_complement = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_explicit_schur_complement"), + solver_options.use_explicit_schur_complement, tmp_descr); #if !CERES_VERSION_AT_LEAST(2, 2, 0) - tmp_descr.description = ( - "In some rare cases, it is worth using a more complicated " - "reordering algorithm which has slightly better runtime " - "performance at the expense of an extra copy of the Jacobian " - "matrix. Setting use_postordering to true enables this tradeoff."); + tmp_descr.description = ("In some rare cases, it is worth using a more complicated " + "reordering algorithm which has slightly better runtime " + "performance at the expense of an extra copy of the Jacobian " + "matrix. Setting use_postordering to true enables this tradeoff."); solver_options.use_postordering = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_postordering"), - solver_options.use_postordering, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "use_postordering"), solver_options.use_postordering, tmp_descr); #endif tmp_descr.description = "This settings only affects the SPARSE_NORMAL_CHOLESKY solver."; solver_options.dynamic_sparsity = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "dynamic_sparsity"), - solver_options.dynamic_sparsity, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "dynamic_sparsity"), solver_options.dynamic_sparsity, tmp_descr); #if CERES_VERSION_AT_LEAST(2, 0, 0) - tmp_descr.description = ( - "NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " - "\n" - "If use_mixed_precision_solves is true, the Gauss-Newton matrix " - "is computed in double precision, but its factorization is " - "computed in single precision. This can result in significant " - "time and memory savings at the cost of some accuracy in the " - "Gauss-Newton step. Iterative refinement is used to recover some " - "of this accuracy back."); - solver_options.use_mixed_precision_solves = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_mixed_precision_solves"), - solver_options.use_mixed_precision_solves, - tmp_descr - ); + tmp_descr.description = ("NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " + "\n" + "If use_mixed_precision_solves is true, the Gauss-Newton matrix " + "is computed in double precision, but its factorization is " + "computed in single precision. This can result in significant " + "time and memory savings at the cost of some accuracy in the " + "Gauss-Newton step. Iterative refinement is used to recover some " + "of this accuracy back."); + solver_options.use_mixed_precision_solves = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_mixed_precision_solves"), + solver_options.use_mixed_precision_solves, tmp_descr); tmp_descr.description = - "Number steps of the iterative refinement process to run when computing the Gauss-Newton step."; - solver_options.max_num_refinement_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_refinement_iterations"), - solver_options.max_num_refinement_iterations, - tmp_descr - ); + "Number steps of the iterative refinement process to run when computing the Gauss-Newton step."; + solver_options.max_num_refinement_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_refinement_iterations"), + solver_options.max_num_refinement_iterations, tmp_descr); #endif #if CERES_VERSION_AT_LEAST(2, 2, 0) - tmp_descr.description = ( - "Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION. " - "Each iteration corresponds to one more term in the power series expansion " - "of the inverse of the Schur complement. This value controls the maximum " - "number of iterations whether it is used as a preconditioner or just to " - "initialize the solution for ITERATIVE_SCHUR."); - solver_options.max_num_spse_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_spse_iterations"), - solver_options.max_num_spse_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for " - "ITERATIVE_SCHUR. This option can be set true regardless of what " - "preconditioner is being used."); - solver_options.use_spse_initialization = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_spse_initialization"), - solver_options.use_spse_initialization, - tmp_descr - ); - - tmp_descr.description = ( - "When use_spse_initialization is true, this parameter along with " - "max_num_spse_iterations controls the number of " - "SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It " - "is not used to control the preconditioner."); - solver_options.spse_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "spse_tolerance"), - solver_options.spse_tolerance, - tmp_descr - ); + tmp_descr.description = ("Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION. " + "Each iteration corresponds to one more term in the power series expansion " + "of the inverse of the Schur complement. This value controls the maximum " + "number of iterations whether it is used as a preconditioner or just to " + "initialize the solution for ITERATIVE_SCHUR."); + solver_options.max_num_spse_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_spse_iterations"), + solver_options.max_num_spse_iterations, tmp_descr); + + tmp_descr.description = ("Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for " + "ITERATIVE_SCHUR. This option can be set true regardless of what " + "preconditioner is being used."); + solver_options.use_spse_initialization = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_spse_initialization"), + solver_options.use_spse_initialization, tmp_descr); + + tmp_descr.description = ("When use_spse_initialization is true, this parameter along with " + "max_num_spse_iterations controls the number of " + "SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It " + "is not used to control the preconditioner."); + solver_options.spse_tolerance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "spse_tolerance"), + solver_options.spse_tolerance, tmp_descr); #endif - tmp_descr.description = - "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; - solver_options.use_inner_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_inner_iterations"), - solver_options.use_inner_iterations, - tmp_descr - ); + tmp_descr.description = "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; + solver_options.use_inner_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_inner_iterations"), + solver_options.use_inner_iterations, tmp_descr); // No parameter is loaded for: std::shared_ptr inner_iteration_ordering; - tmp_descr.description = ( - "Once the relative decrease in the objective function due to " - "inner iterations drops below inner_iteration_tolerance, the use " - "of inner iterations in subsequent trust region minimizer " - "iterations is disabled."); - solver_options.inner_iteration_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "inner_iteration_tolerance"), - solver_options.inner_iteration_tolerance, - tmp_descr - ); + tmp_descr.description = ("Once the relative decrease in the objective function due to " + "inner iterations drops below inner_iteration_tolerance, the use " + "of inner iterations in subsequent trust region minimizer " + "iterations is disabled."); + solver_options.inner_iteration_tolerance = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "inner_iteration_tolerance"), + solver_options.inner_iteration_tolerance, tmp_descr); tmp_descr.description = "Minimum number of iterations used by the linear iterative solver"; - solver_options.min_linear_solver_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_linear_solver_iterations"), - solver_options.min_linear_solver_iterations, - tmp_descr - ); + solver_options.min_linear_solver_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_linear_solver_iterations"), + solver_options.min_linear_solver_iterations, tmp_descr); tmp_descr.description = "Maximum number of iterations used by the linear iterative solver"; - solver_options.max_linear_solver_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_linear_solver_iterations"), - solver_options.max_linear_solver_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Forcing sequence parameter. The truncated Newton solver uses this number to control the " - "relative accuracy with which the Newton step is computed"); - solver_options.eta = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "eta"), - solver_options.eta, - tmp_descr - ); - - tmp_descr.description = ( - "True means that the Jacobian is scaled by the norm of its columns before being passed to the " - "linear solver. This improves the numerical conditioning of the normal equations"); - solver_options.jacobi_scaling = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "jacobi_scaling"), - solver_options.jacobi_scaling, - tmp_descr - ); + solver_options.max_linear_solver_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_linear_solver_iterations"), + solver_options.max_linear_solver_iterations, tmp_descr); + + tmp_descr.description = ("Forcing sequence parameter. The truncated Newton solver uses this number to control the " + "relative accuracy with which the Newton step is computed"); + solver_options.eta = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "eta"), solver_options.eta, tmp_descr); + + tmp_descr.description = + ("True means that the Jacobian is scaled by the norm of its columns before being passed to the " + "linear solver. This improves the numerical conditioning of the normal equations"); + solver_options.jacobi_scaling = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "jacobi_scaling"), + solver_options.jacobi_scaling, tmp_descr); // Logging options solver_options.logging_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "logging_type"), solver_options.logging_type); + interfaces, fuse_core::joinParameterName(ns, "logging_type"), solver_options.logging_type); tmp_descr.description = "If logging_type is not SILENT, sends the logging output to STDOUT"; - solver_options.minimizer_progress_to_stdout = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "minimizer_progress_to_stdout"), - solver_options.minimizer_progress_to_stdout, - tmp_descr - ); + solver_options.minimizer_progress_to_stdout = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "minimizer_progress_to_stdout"), + solver_options.minimizer_progress_to_stdout, tmp_descr); fuse_core::getParam>( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump"), - std::vector() - ); - std::vector iterations_to_dump_tmp = interfaces.get_node_parameters_interface() - ->get_parameter(fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump")) - .get_value>(); - if (!iterations_to_dump_tmp.empty()) { + interfaces, fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump"), std::vector()); + std::vector iterations_to_dump_tmp = + interfaces.get_node_parameters_interface() + ->get_parameter(fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump")) + .get_value>(); + if (!iterations_to_dump_tmp.empty()) + { solver_options.trust_region_minimizer_iterations_to_dump.reserve(iterations_to_dump_tmp.size()); - std::transform( - iterations_to_dump_tmp.begin(), - iterations_to_dump_tmp.end(), - std::back_inserter(solver_options.trust_region_minimizer_iterations_to_dump), - [](int64_t val) {return val;}); + std::transform(iterations_to_dump_tmp.begin(), iterations_to_dump_tmp.end(), + std::back_inserter(solver_options.trust_region_minimizer_iterations_to_dump), + [](int64_t val) { return val; }); } - tmp_descr.description = ( - "Directory to which the problems should be written to. Should be " - "non-empty if trust_region_minimizer_iterations_to_dump is " - "non-empty and trust_region_problem_dump_format_type is not " - "CONSOLE."); - solver_options.trust_region_problem_dump_directory = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_problem_dump_directory"), - solver_options.trust_region_problem_dump_directory, - tmp_descr - ); + tmp_descr.description = ("Directory to which the problems should be written to. Should be " + "non-empty if trust_region_minimizer_iterations_to_dump is " + "non-empty and trust_region_problem_dump_format_type is not " + "CONSOLE."); + solver_options.trust_region_problem_dump_directory = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "trust_region_problem_dump_directory"), + solver_options.trust_region_problem_dump_directory, tmp_descr); solver_options.trust_region_problem_dump_format_type = - fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_problem_dump_format_type"), - solver_options.trust_region_problem_dump_format_type - ); + fuse_core::declareCeresParam(interfaces, + fuse_core::joinParameterName(ns, "trust_region_problem_dump_format_type"), + solver_options.trust_region_problem_dump_format_type); // Finite differences options - tmp_descr.description = ( - "Check all Jacobians computed by each residual block with finite differences, abort if numeric " - "and analytic gradients differ substantially)"); - solver_options.check_gradients = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "check_gradients"), - solver_options.check_gradients, - tmp_descr - ); - tmp_descr.description = ( - "Precision to check for in the gradient checker. If the relative " - "difference between an element in a Jacobian exceeds this number, then the Jacobian for that " - "cost term is dumped"); - solver_options.gradient_check_relative_precision = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_check_relative_precision"), - solver_options.gradient_check_relative_precision, - tmp_descr - ); - tmp_descr.description = ( - "Relative shift used for taking numeric derivatives when " - "Solver::Options::check_gradients is true."); - solver_options.gradient_check_numeric_derivative_relative_step_size = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_check_numeric_derivative_relative_step_size"), - solver_options.gradient_check_numeric_derivative_relative_step_size, - tmp_descr - ); - tmp_descr.description = ( - "If update_state_every_iteration is true, then Ceres Solver will guarantee that at the end of " - "every iteration and before any user IterationCallback is called, the parameter blocks are " - "updated to the current best solution found by the solver. Thus the IterationCallback can " - "inspect the values of the parameter blocks for purposes of computation, visualization or " - "termination"); - solver_options.update_state_every_iteration = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "update_state_every_iteration"), - solver_options.update_state_every_iteration, - tmp_descr - ); + tmp_descr.description = + ("Check all Jacobians computed by each residual block with finite differences, abort if numeric " + "and analytic gradients differ substantially)"); + solver_options.check_gradients = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "check_gradients"), + solver_options.check_gradients, tmp_descr); + tmp_descr.description = + ("Precision to check for in the gradient checker. If the relative " + "difference between an element in a Jacobian exceeds this number, then the Jacobian for that " + "cost term is dumped"); + solver_options.gradient_check_relative_precision = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "gradient_check_relative_precision"), + solver_options.gradient_check_relative_precision, tmp_descr); + tmp_descr.description = ("Relative shift used for taking numeric derivatives when " + "Solver::Options::check_gradients is true."); + solver_options.gradient_check_numeric_derivative_relative_step_size = + fuse_core::getParam(interfaces, + fuse_core::joinParameterName(ns, "gradient_check_numeric_derivative_relative_step_size"), + solver_options.gradient_check_numeric_derivative_relative_step_size, tmp_descr); + tmp_descr.description = + ("If update_state_every_iteration is true, then Ceres Solver will guarantee that at the end of " + "every iteration and before any user IterationCallback is called, the parameter blocks are " + "updated to the current best solution found by the solver. Thus the IterationCallback can " + "inspect the values of the parameter blocks for purposes of computation, visualization or " + "termination"); + solver_options.update_state_every_iteration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "update_state_every_iteration"), + solver_options.update_state_every_iteration, tmp_descr); std::string error; - if (!solver_options.IsValid(&error)) { - throw std::invalid_argument( - "Invalid solver options in parameter " + - std::string(interfaces.get_node_base_interface()->get_namespace()) + - ". Error: " + error); + if (!solver_options.IsValid(&error)) + { + throw std::invalid_argument("Invalid solver options in parameter " + + std::string(interfaces.get_node_base_interface()->get_namespace()) + + ". Error: " + error); } } // NOLINT [readability/fn_size] diff --git a/fuse_core/src/constraint.cpp b/fuse_core/src/constraint.cpp index 89a32ecf4..0a3690cea 100644 --- a/fuse_core/src/constraint.cpp +++ b/fuse_core/src/constraint.cpp @@ -41,14 +41,12 @@ namespace fuse_core { -Constraint::Constraint(const std::string & source, std::initializer_list variable_uuid_list) -: source_(source), - uuid_(uuid::generate()), - variables_(variable_uuid_list) +Constraint::Constraint(const std::string& source, std::initializer_list variable_uuid_list) + : source_(source), uuid_(uuid::generate()), variables_(variable_uuid_list) { } -std::ostream & operator<<(std::ostream & stream, const Constraint & constraint) +std::ostream& operator<<(std::ostream& stream, const Constraint& constraint) { constraint.print(stream); return stream; diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp index 9cf3f4a01..2d2f6c0d9 100644 --- a/fuse_core/src/fuse_echo.cpp +++ b/fuse_core/src/fuse_echo.cpp @@ -50,20 +50,13 @@ namespace fuse_core class FuseEcho : public rclcpp::Node { public: - explicit FuseEcho(rclcpp::NodeOptions options) - : Node("fuse_echo", options) + explicit FuseEcho(rclcpp::NodeOptions options) : Node("fuse_echo", options) { // Subscribe to the constraint topic graph_sub_ = this->create_subscription( - "graph", - rclcpp::QoS(100), - std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1) - ); + "graph", rclcpp::QoS(100), std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1)); transaction_sub_ = this->create_subscription( - "transaction", - rclcpp::QoS(100), - std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1) - ); + "transaction", rclcpp::QoS(100), std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1)); } private: @@ -72,7 +65,7 @@ class FuseEcho : public rclcpp::Node rclcpp::Subscription::SharedPtr graph_sub_; rclcpp::Subscription::SharedPtr transaction_sub_; - void graphCallback(const fuse_msgs::msg::SerializedGraph & msg) + void graphCallback(const fuse_msgs::msg::SerializedGraph& msg) { std::cout << "-------------------------" << std::endl; std::cout << "GRAPH:" << std::endl; @@ -81,7 +74,7 @@ class FuseEcho : public rclcpp::Node graph->print(); } - void transactionCallback(const fuse_msgs::msg::SerializedTransaction & msg) + void transactionCallback(const fuse_msgs::msg::SerializedTransaction& msg) { std::cout << "-------------------------" << std::endl; std::cout << "TRANSACTION:" << std::endl; @@ -93,7 +86,6 @@ class FuseEcho : public rclcpp::Node } // namespace fuse_core - #include RCLCPP_COMPONENTS_REGISTER_NODE(fuse_core::FuseEcho) diff --git a/fuse_core/src/graph.cpp b/fuse_core/src/graph.cpp index 06ecbd9b2..ddd9cf153 100644 --- a/fuse_core/src/graph.cpp +++ b/fuse_core/src/graph.cpp @@ -41,48 +41,48 @@ namespace fuse_core { -std::ostream & operator<<(std::ostream & stream, const Graph & graph) +std::ostream& operator<<(std::ostream& stream, const Graph& graph) { graph.print(stream); return stream; } -Graph::const_variable_range Graph::getConnectedVariables(const UUID & constraint_uuid) const +Graph::const_variable_range Graph::getConnectedVariables(const UUID& constraint_uuid) const { - std::function uuid_to_variable_ref = - [this](const UUID & variable_uuid) -> const Variable & - { - return this->getVariable(variable_uuid); - }; + std::function uuid_to_variable_ref = + [this](const UUID& variable_uuid) -> const Variable& { return this->getVariable(variable_uuid); }; - const auto & constraint = getConstraint(constraint_uuid); - const auto & variable_uuids = constraint.variables(); + const auto& constraint = getConstraint(constraint_uuid); + const auto& variable_uuids = constraint.variables(); - return const_variable_range( - boost::make_transform_iterator(variable_uuids.cbegin(), uuid_to_variable_ref), - boost::make_transform_iterator(variable_uuids.cend(), uuid_to_variable_ref)); + return const_variable_range(boost::make_transform_iterator(variable_uuids.cbegin(), uuid_to_variable_ref), + boost::make_transform_iterator(variable_uuids.cend(), uuid_to_variable_ref)); } -void Graph::update(const Transaction & transaction) +void Graph::update(const Transaction& transaction) { // Update the graph with a new transaction. In order to keep the graph consistent, variables are // added first, followed by the constraints which might use the newly added variables. Then // constraints are removed so that the variable usage is updated. Finally, variables are removed. // Insert the new variables into the graph - for (const auto & variable : transaction.addedVariables()) { + for (const auto& variable : transaction.addedVariables()) + { addVariable(variable.clone()); } // Insert the new constraints into the graph - for (const auto & constraint : transaction.addedConstraints()) { + for (const auto& constraint : transaction.addedConstraints()) + { addConstraint(constraint.clone()); } // Delete constraints from the graph - for (const auto & constraint_uuid : transaction.removedConstraints()) { + for (const auto& constraint_uuid : transaction.removedConstraints()) + { removeConstraint(constraint_uuid); } // Delete variables from the graph - for (const auto & variable_uuid : transaction.removedVariables()) { + for (const auto& variable_uuid : transaction.removedVariables()) + { removeVariable(variable_uuid); } } diff --git a/fuse_core/src/graph_deserializer.cpp b/fuse_core/src/graph_deserializer.cpp index e21e9e94c..76fb19d53 100644 --- a/fuse_core/src/graph_deserializer.cpp +++ b/fuse_core/src/graph_deserializer.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGraph & msg) +void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg) { // Serialize the graph into the msg.data field boost::iostreams::stream stream(msg.data); @@ -54,27 +54,29 @@ void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGr } GraphDeserializer::GraphDeserializer() -: variable_loader_("fuse_core", "fuse_core::Variable"), - constraint_loader_("fuse_core", "fuse_core::Constraint"), - loss_loader_("fuse_core", "fuse_core::Loss"), - graph_loader_("fuse_core", "fuse_core::Graph") + : variable_loader_("fuse_core", "fuse_core::Variable") + , constraint_loader_("fuse_core", "fuse_core::Constraint") + , loss_loader_("fuse_core", "fuse_core::Loss") + , graph_loader_("fuse_core", "fuse_core::Graph") { // Load all known plugin libraries // I believe the library containing a given Variable or Constraint type must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto & class_name : variable_loader_.getDeclaredClasses()) { + for (const auto& class_name : variable_loader_.getDeclaredClasses()) + { variable_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : constraint_loader_.getDeclaredClasses()) { + for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : loss_loader_.getDeclaredClasses()) { + for (const auto& class_name : loss_loader_.getDeclaredClasses()) + { loss_loader_.loadLibraryForClass(class_name); } } -fuse_core::Graph::UniquePtr GraphDeserializer::deserialize( - const fuse_msgs::msg::SerializedGraph & msg) const +fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::msg::SerializedGraph& msg) const { // Create a Graph object using pluginlib. This will throw if the plugin name is not found. // The unique ptr returned by pluginlib has a custom deleter. This makes it annoying to return diff --git a/fuse_core/src/loss.cpp b/fuse_core/src/loss.cpp index 44e0e8311..328cf3fb6 100644 --- a/fuse_core/src/loss.cpp +++ b/fuse_core/src/loss.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -std::ostream & operator<<(std::ostream & stream, const Loss & loss) +std::ostream& operator<<(std::ostream& stream, const Loss& loss) { loss.print(stream); return stream; diff --git a/fuse_core/src/parameter.cpp b/fuse_core/src/parameter.cpp index a2d01a2d7..2ffc13446 100644 --- a/fuse_core/src/parameter.cpp +++ b/fuse_core/src/parameter.cpp @@ -38,43 +38,44 @@ #include #include - namespace fuse_core { std::unordered_set -list_parameter_override_prefixes( - node_interfaces::NodeInterfaces interfaces, - std::string prefix) +list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, + std::string prefix) { - const std::map & overrides = - interfaces.get_node_parameters_interface()->get_parameter_overrides(); + const std::map& overrides = + interfaces.get_node_parameters_interface()->get_parameter_overrides(); return detail::list_parameter_override_prefixes(overrides, prefix); } std::unordered_set -detail::list_parameter_override_prefixes( - const std::map & overrides, - std::string prefix) +detail::list_parameter_override_prefixes(const std::map& overrides, + std::string prefix) { // TODO(sloretz) ROS 2 must have this in a header somewhere, right? const char kParamSeparator = '.'; // Find all overrides starting with "prefix.", unless the prefix is empty. // If the prefix is empty then look at all parameter overrides. - if (!prefix.empty() && prefix.back() != kParamSeparator) { + if (!prefix.empty() && prefix.back() != kParamSeparator) + { prefix += kParamSeparator; } std::unordered_set output_names; - for (const auto & kv : overrides) { - const std::string & name = kv.first; - if (name.size() <= prefix.size()) { + for (const auto& kv : overrides) + { + const std::string& name = kv.first; + if (name.size() <= prefix.size()) + { // Too short, no point in checking continue; } assert(prefix.size() < name.size()); // TODO(sloretz) use string::starts_with in c++20 - if (name.rfind(prefix, 0) == 0) { // if name starts with prefix + if (name.rfind(prefix, 0) == 0) + { // if name starts with prefix // Truncate names to the next separator size_t separator_pos = name.find(kParamSeparator, prefix.size()); // Insert truncated name @@ -84,18 +85,22 @@ detail::list_parameter_override_prefixes( return output_names; } -std::string joinParameterName(const std::string & left, const std::string & right) +std::string joinParameterName(const std::string& left, const std::string& right) { - if (left.empty()) { + if (left.empty()) + { return right; } - if (right.empty()) { + if (right.empty()) + { return left; } - if ('.' != left.back() && '.' != right.front() ) { + if ('.' != left.back() && '.' != right.front()) + { return left + '.' + right; } - if ('.' == left.back() && '.' == right.front()) { + if ('.' == left.back() && '.' == right.front()) + { return left + right.substr(1); } return left + right; diff --git a/fuse_core/src/serialization.cpp b/fuse_core/src/serialization.cpp index 36bf7af60..3debb1b39 100644 --- a/fuse_core/src/serialization.cpp +++ b/fuse_core/src/serialization.cpp @@ -39,30 +39,30 @@ namespace fuse_core { -MessageBufferStreamSource::MessageBufferStreamSource(const std::vector & data) -: data_(data), - index_(0) +MessageBufferStreamSource::MessageBufferStreamSource(const std::vector& data) : data_(data), index_(0) { } -std::streamsize MessageBufferStreamSource::read(char_type * s, std::streamsize n) +std::streamsize MessageBufferStreamSource::read(char_type* s, std::streamsize n) { std::streamsize result = std::min(n, static_cast(data_.size() - index_)); - if (result != 0) { + if (result != 0) + { std::copy(data_.begin() + index_, data_.begin() + index_ + result, s); index_ += result; return result; - } else { + } + else + { return -1; // EOF } } -MessageBufferStreamSink::MessageBufferStreamSink(std::vector & data) -: data_(data) +MessageBufferStreamSink::MessageBufferStreamSink(std::vector& data) : data_(data) { } -std::streamsize MessageBufferStreamSink::write(const char_type * s, std::streamsize n) +std::streamsize MessageBufferStreamSink::write(const char_type* s, std::streamsize n) { data_.insert(data_.end(), s, s + n); return n; diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index cfcfdecd2..80031b584 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -43,31 +43,25 @@ namespace fuse_core { -TimestampManager::TimestampManager( - MotionModelFunction generator, - const rclcpp::Duration & buffer_length) -: generator_(generator), - buffer_length_(buffer_length) +TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) + : generator_(generator), buffer_length_(buffer_length) { } -void TimestampManager::query( - Transaction & transaction, - bool update_variables) +void TimestampManager::query(Transaction& transaction, bool update_variables) { // Handle the trivial cases first - const auto & stamps = transaction.involvedStamps(); - if (stamps.empty()) { + const auto& stamps = transaction.involvedStamps(); + if (stamps.empty()) + { return; } // Verify the query is within the buffer length - if ( (!motion_model_history_.empty()) && - (buffer_length_ != rclcpp::Duration::max()) && - (stamps.front() < motion_model_history_.begin()->first) && - (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) + if ((!motion_model_history_.empty()) && (buffer_length_ != rclcpp::Duration::max()) && + (stamps.front() < motion_model_history_.begin()->first) && + (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) { - throw std::invalid_argument( - "All timestamps must be within the defined buffer length of the motion model"); + throw std::invalid_argument("All timestamps must be within the defined buffer length of the motion model"); } // Create a list of all the required timestamps involved in motion model segments that must be // created Add all of the existing timestamps between the first and last input stamp @@ -77,46 +71,45 @@ void TimestampManager::query( auto last_stamp = *augmented_stamps.rbegin(); { auto begin = motion_model_history_.upper_bound(first_stamp); - if (begin != motion_model_history_.begin()) { + if (begin != motion_model_history_.begin()) + { --begin; } auto end = motion_model_history_.upper_bound(last_stamp); - for (auto iter = begin; iter != end; ++iter) { + for (auto iter = begin; iter != end; ++iter) + { augmented_stamps.insert(iter->first); } - if (end != motion_model_history_.end()) { + if (end != motion_model_history_.end()) + { augmented_stamps.insert(end->first); } } // Convert the sequence of stamps into stamp pairs that must be generated std::vector> stamp_pairs; { - for (auto previous_iter = augmented_stamps.begin(), - current_iter = std::next(augmented_stamps.begin()); - current_iter != augmented_stamps.end(); - ++previous_iter, ++current_iter) + for (auto previous_iter = augmented_stamps.begin(), current_iter = std::next(augmented_stamps.begin()); + current_iter != augmented_stamps.end(); ++previous_iter, ++current_iter) { - const rclcpp::Time & previous_stamp = *previous_iter; - const rclcpp::Time & current_stamp = *current_iter; + const rclcpp::Time& previous_stamp = *previous_iter; + const rclcpp::Time& current_stamp = *current_iter; // Check if the timestamp pair is exactly an existing pair. If so, don't add it. auto history_iter = motion_model_history_.lower_bound(previous_stamp); - if ((history_iter != motion_model_history_.end()) && - (history_iter->second.beginning_stamp == previous_stamp) && - (history_iter->second.ending_stamp == current_stamp)) + if ((history_iter != motion_model_history_.end()) && (history_iter->second.beginning_stamp == previous_stamp) && + (history_iter->second.ending_stamp == current_stamp)) { - if (update_variables) { + if (update_variables) + { // Add the motion model version of the variables involved in this motion model segment // This ensures that the variables in the final transaction will be overwritten with the // motion model version auto transaction_variables = transaction.addedVariables(); - for (const auto & variable : history_iter->second.variables) { - if (std::any_of( - transaction_variables.begin(), - transaction_variables.end(), - [variable_uuid = variable->uuid()](const auto & input_variable) - { - return input_variable.uuid() == variable_uuid; - })) // NOLINT + for (const auto& variable : history_iter->second.variables) + { + if (std::any_of(transaction_variables.begin(), transaction_variables.end(), + [variable_uuid = variable->uuid()](const auto& input_variable) { + return input_variable.uuid() == variable_uuid; + })) // NOLINT { motion_model_transaction.addVariable(variable, update_variables); } @@ -125,9 +118,8 @@ void TimestampManager::query( continue; } // Check if this stamp is in the middle of an existing entry. If so, delete it. - if ((history_iter != motion_model_history_.end()) && - (history_iter->second.beginning_stamp < current_stamp) && - (history_iter->second.ending_stamp >= current_stamp)) + if ((history_iter != motion_model_history_.end()) && (history_iter->second.beginning_stamp < current_stamp) && + (history_iter->second.ending_stamp >= current_stamp)) { removeSegment(history_iter, motion_model_transaction); } @@ -136,12 +128,15 @@ void TimestampManager::query( } } // Create the required segments - for (const auto & stamp_pair : stamp_pairs) { + for (const auto& stamp_pair : stamp_pairs) + { addSegment(stamp_pair.first, stamp_pair.second, motion_model_transaction); } // Add a dummy entry for the last stamp if one does not already exist - if (motion_model_history_.empty() || (motion_model_history_.rbegin()->first < last_stamp)) { - if (motion_model_history_.empty()) { + if (motion_model_history_.empty() || (motion_model_history_.rbegin()->first < last_stamp)) + { + if (motion_model_history_.empty()) + { // Call the motion model generator so it inserts the last timestamp into its state history. std::vector constraints; std::vector variables; @@ -160,21 +155,15 @@ void TimestampManager::query( TimestampManager::const_stamp_range TimestampManager::stamps() const { - std::function extract_stamp = - [](const MotionModelHistory::value_type & element) -> const rclcpp::Time & - { - return element.first; - }; + std::function extract_stamp = + [](const MotionModelHistory::value_type& element) -> const rclcpp::Time& { return element.first; }; - return const_stamp_range( - boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), - boost::make_transform_iterator(motion_model_history_.end(), extract_stamp)); + return const_stamp_range(boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), + boost::make_transform_iterator(motion_model_history_.end(), extract_stamp)); } -void TimestampManager::addSegment( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - Transaction & transaction) +void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + Transaction& transaction) { // Generate the set of constraints and variables to add std::vector constraints; @@ -183,28 +172,25 @@ void TimestampManager::addSegment( // Update the transaction with the generated constraints/variables transaction.addInvolvedStamp(beginning_stamp); transaction.addInvolvedStamp(ending_stamp); - for (const auto & constraint : constraints) { + for (const auto& constraint : constraints) + { transaction.addConstraint(constraint); } - for (const auto & variable : variables) { + for (const auto& variable : variables) + { transaction.addVariable(variable); } - motion_model_history_.insert_or_assign( - beginning_stamp, MotionModelSegment( - beginning_stamp, - ending_stamp, - constraints, - variables)); + motion_model_history_.insert_or_assign(beginning_stamp, + MotionModelSegment(beginning_stamp, ending_stamp, constraints, variables)); } -void TimestampManager::removeSegment( - MotionModelHistory::iterator & iter, - Transaction & transaction) +void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transaction& transaction) { // Mark the previously generated constraints for removal transaction.addInvolvedStamp(iter->second.beginning_stamp); transaction.addInvolvedStamp(iter->second.ending_stamp); - for (const auto & constraint : iter->second.constraints) { + for (const auto& constraint : iter->second.constraints) + { transaction.removeConstraint(constraint->uuid()); } // We do not remove variables here. It is assumed the variables are still in use by other @@ -214,10 +200,8 @@ void TimestampManager::removeSegment( motion_model_history_.erase(iter); } -void TimestampManager::splitSegment( - MotionModelHistory::iterator & iter, - const rclcpp::Time & stamp, - Transaction & transaction) +void TimestampManager::splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, + Transaction& transaction) { rclcpp::Time removed_beginning_stamp = iter->second.beginning_stamp; rclcpp::Time removed_ending_stamp = iter->second.ending_stamp; @@ -234,7 +218,8 @@ void TimestampManager::purgeHistory() // Purge any motion model segments that are more than buffer_length_ seconds older than the most // recent entry A setting of rclcpp::Duration::max() means "keep everything" And we want to keep // at least one entry in motion model history, regardless of the stamps. - if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1)) { + if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1)) + { return; } // Continue to remove the first entry from the history until we: @@ -242,8 +227,8 @@ void TimestampManager::purgeHistory() // (b) the time delta between the beginning and end is within the buffer_length_ We compare with // the ending timestamp of each segment to be conservative rclcpp::Time ending_stamp = motion_model_history_.rbegin()->first; - while ( (motion_model_history_.size() > 1) && - ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) + while ((motion_model_history_.size() > 1) && + ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) { motion_model_history_.erase(motion_model_history_.begin()); } diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index cbee6c839..dc1c088a6 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -41,40 +41,42 @@ namespace fuse_core { -const rclcpp::Time & Transaction::minStamp() const +const rclcpp::Time& Transaction::minStamp() const { - if (involved_stamps_.empty()) { + if (involved_stamps_.empty()) + { return stamp_; - } else { + } + else + { return std::min(*involved_stamps_.begin(), stamp_); } } -const rclcpp::Time & Transaction::maxStamp() const +const rclcpp::Time& Transaction::maxStamp() const { - if (involved_stamps_.empty()) { + if (involved_stamps_.empty()) + { return stamp_; - } else { + } + else + { return std::max(*involved_stamps_.rbegin(), stamp_); } } -void Transaction::addInvolvedStamp(const rclcpp::Time & stamp) +void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) { involved_stamps_.insert(stamp); } Transaction::const_constraint_range Transaction::addedConstraints() const { - std::function to_constraint_ref = - [](const Constraint::SharedPtr & constraint)->const Constraint & - { - return *constraint; - }; + std::function to_constraint_ref = + [](const Constraint::SharedPtr& constraint) -> const Constraint& { return *constraint; }; - return const_constraint_range( - boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref)); + return const_constraint_range(boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref)); } void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite) @@ -82,49 +84,45 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite // If the constraint being added is in the 'removed' container, then delete it from the 'removed' // container instead of adding it to the 'added' container. UUID constraint_uuid = constraint->uuid(); - auto removed_constraints_iter = std::find( - removed_constraints_.begin(), - removed_constraints_.end(), constraint_uuid); - if (removed_constraints_iter != removed_constraints_.end()) { + auto removed_constraints_iter = std::find(removed_constraints_.begin(), removed_constraints_.end(), constraint_uuid); + if (removed_constraints_iter != removed_constraints_.end()) + { removed_constraints_.erase(removed_constraints_iter); return; } // Also don't add the same constraint twice - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; - auto added_constraints_iter = std::find_if( - added_constraints_.begin(), - added_constraints_.end(), is_constraint_added); - if (added_constraints_iter == added_constraints_.end()) { + auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + return constraint_uuid == added_constraint->uuid(); + }; + auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); + if (added_constraints_iter == added_constraints_.end()) + { added_constraints_.push_back(std::move(constraint)); - } else if (overwrite) { + } + else if (overwrite) + { *added_constraints_iter = std::move(constraint); } } -void Transaction::removeConstraint(const UUID & constraint_uuid) +void Transaction::removeConstraint(const UUID& constraint_uuid) { // If the constraint being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; - auto added_constraints_iter = std::find_if( - added_constraints_.begin(), - added_constraints_.end(), is_constraint_added); - if (added_constraints_iter != added_constraints_.end()) { + auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + return constraint_uuid == added_constraint->uuid(); + }; + auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); + if (added_constraints_iter != added_constraints_.end()) + { added_constraints_.erase(added_constraints_iter); return; } // Also don't remove the same constraint twice - auto removed_constraints_iter = std::find( - removed_constraints_.begin(), - removed_constraints_.end(), constraint_uuid); - if (removed_constraints_iter == removed_constraints_.end()) { + auto removed_constraints_iter = std::find(removed_constraints_.begin(), removed_constraints_.end(), constraint_uuid); + if (removed_constraints_iter == removed_constraints_.end()) + { removed_constraints_.push_back(constraint_uuid); return; } @@ -132,22 +130,17 @@ void Transaction::removeConstraint(const UUID & constraint_uuid) Transaction::const_variable_range Transaction::addedVariables() const { - std::function to_variable_ref = - [](const Variable::SharedPtr & variable)->const Variable & - { - return *variable; - }; + std::function to_variable_ref = + [](const Variable::SharedPtr& variable) -> const Variable& { return *variable; }; - return const_variable_range( - boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(added_variables_.cend(), to_variable_ref)); + return const_variable_range(boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(added_variables_.cend(), to_variable_ref)); } bool Transaction::empty() const { - return boost::empty(added_variables_) && boost::empty(removed_variables_) && - boost::empty(added_constraints_) && boost::empty(removed_constraints_) && - involved_stamps_.empty(); + return boost::empty(added_variables_) && boost::empty(removed_variables_) && boost::empty(added_constraints_) && + boost::empty(removed_constraints_) && involved_stamps_.empty(); } void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) @@ -156,94 +149,99 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) // container instead of adding it to the 'added' container. UUID variable_uuid = variable->uuid(); - auto removed_variables_iter = std::find( - removed_variables_.begin(), - removed_variables_.end(), variable_uuid); - if (removed_variables_iter != removed_variables_.end()) { + auto removed_variables_iter = std::find(removed_variables_.begin(), removed_variables_.end(), variable_uuid); + if (removed_variables_iter != removed_variables_.end()) + { removed_variables_.erase(removed_variables_iter); return; } // Also don't add the same variable twice - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; - auto added_variables_iter = std::find_if( - added_variables_.begin(), - added_variables_.end(), is_variable_added); - if (added_variables_iter == added_variables_.end()) { + auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + return variable_uuid == added_variable->uuid(); + }; + auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); + if (added_variables_iter == added_variables_.end()) + { added_variables_.push_back(std::move(variable)); - } else if (overwrite) { + } + else if (overwrite) + { *added_variables_iter = std::move(variable); } } -void Transaction::removeVariable(const UUID & variable_uuid) +void Transaction::removeVariable(const UUID& variable_uuid) { // If the variable being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; - auto added_variables_iter = std::find_if( - added_variables_.begin(), - added_variables_.end(), is_variable_added); - if (added_variables_iter != added_variables_.end()) { + auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + return variable_uuid == added_variable->uuid(); + }; + auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); + if (added_variables_iter != added_variables_.end()) + { added_variables_.erase(added_variables_iter); return; } // Also don't remove the same variable twice - auto removed_variables_iter = std::find( - removed_variables_.begin(), - removed_variables_.end(), variable_uuid); - if (removed_variables_iter == removed_variables_.end()) { + auto removed_variables_iter = std::find(removed_variables_.begin(), removed_variables_.end(), variable_uuid); + if (removed_variables_iter == removed_variables_.end()) + { removed_variables_.push_back(variable_uuid); return; } } -void Transaction::merge(const Transaction & other, bool overwrite) +void Transaction::merge(const Transaction& other, bool overwrite) { stamp_ = std::max(stamp_, other.stamp_); involved_stamps_.insert(other.involved_stamps_.begin(), other.involved_stamps_.end()); - for (const auto & added_constraint : other.added_constraints_) { + for (const auto& added_constraint : other.added_constraints_) + { addConstraint(added_constraint, overwrite); } - for (const auto & removed_constraint : other.removed_constraints_) { + for (const auto& removed_constraint : other.removed_constraints_) + { removeConstraint(removed_constraint); } - for (const auto & added_variable : other.added_variables_) { + for (const auto& added_variable : other.added_variables_) + { addVariable(added_variable, overwrite); } - for (const auto & removed_variable : other.removed_variables_) { + for (const auto& removed_variable : other.removed_variables_) + { removeVariable(removed_variable); } } -void Transaction::print(std::ostream & stream) const +void Transaction::print(std::ostream& stream) const { stream << "Stamp: " << stamp_.nanoseconds() << "\n"; stream << "Involved Timestamps:\n"; - for (const auto & involved_stamp : involved_stamps_) { + for (const auto& involved_stamp : involved_stamps_) + { stream << " - " << involved_stamp.nanoseconds() << "\n"; } stream << "Added Variables:\n"; - for (const auto & added_variable : added_variables_) { + for (const auto& added_variable : added_variables_) + { stream << " - " << *added_variable << "\n"; } stream << "Added Constraints:\n"; - for (const auto & added_constraint : added_constraints_) { + for (const auto& added_constraint : added_constraints_) + { stream << " - " << *added_constraint << "\n"; } stream << "Removed Variables:\n"; - for (const auto & removed_variable : removed_variables_) { + for (const auto& removed_variable : removed_variables_) + { stream << " - " << removed_variable << "\n"; } stream << "Removed Constraints:\n"; - for (const auto & removed_constraint : removed_constraints_) { + for (const auto& removed_constraint : removed_constraints_) + { stream << " - " << removed_constraint << "\n"; } } @@ -253,27 +251,27 @@ Transaction::UniquePtr Transaction::clone() const return Transaction::make_unique(*this); } -void Transaction::serialize(fuse_core::BinaryOutputArchive & archive) const +void Transaction::serialize(fuse_core::BinaryOutputArchive& archive) const { archive << *this; } -void Transaction::serialize(fuse_core::TextOutputArchive & archive) const +void Transaction::serialize(fuse_core::TextOutputArchive& archive) const { archive << *this; } -void Transaction::deserialize(fuse_core::BinaryInputArchive & archive) +void Transaction::deserialize(fuse_core::BinaryInputArchive& archive) { archive >> *this; } -void Transaction::deserialize(fuse_core::TextInputArchive & archive) +void Transaction::deserialize(fuse_core::TextInputArchive& archive) { archive >> *this; } -std::ostream & operator<<(std::ostream & stream, const Transaction & transaction) +std::ostream& operator<<(std::ostream& stream, const Transaction& transaction) { transaction.print(stream); return stream; diff --git a/fuse_core/src/transaction_deserializer.cpp b/fuse_core/src/transaction_deserializer.cpp index b2c1f82d4..9308b5c9f 100644 --- a/fuse_core/src/transaction_deserializer.cpp +++ b/fuse_core/src/transaction_deserializer.cpp @@ -38,9 +38,7 @@ namespace fuse_core { -void serializeTransaction( - const fuse_core::Transaction & transaction, - fuse_msgs::msg::SerializedTransaction & msg) +void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg) { // Serialize the transaction into the msg.data field boost::iostreams::stream stream(msg.data); @@ -53,26 +51,29 @@ void serializeTransaction( } TransactionDeserializer::TransactionDeserializer() -: variable_loader_("fuse_core", "fuse_core::Variable"), - constraint_loader_("fuse_core", "fuse_core::Constraint"), - loss_loader_("fuse_core", "fuse_core::Loss") + : variable_loader_("fuse_core", "fuse_core::Variable") + , constraint_loader_("fuse_core", "fuse_core::Constraint") + , loss_loader_("fuse_core", "fuse_core::Loss") { // Load all known plugin libraries // I believe the library containing a given Variable or Constraint must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto & class_name : variable_loader_.getDeclaredClasses()) { + for (const auto& class_name : variable_loader_.getDeclaredClasses()) + { variable_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : constraint_loader_.getDeclaredClasses()) { + for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : loss_loader_.getDeclaredClasses()) { + for (const auto& class_name : loss_loader_.getDeclaredClasses()) + { loss_loader_.loadLibraryForClass(class_name); } } -fuse_core::Transaction::UniquePtr TransactionDeserializer::deserialize( - const fuse_msgs::msg::SerializedTransaction & msg) const +fuse_core::Transaction::UniquePtr +TransactionDeserializer::deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const { // The Transaction object is not a plugin and has no derived types. That makes it much easier to // use. diff --git a/fuse_core/src/uuid.cpp b/fuse_core/src/uuid.cpp index 608d3d4a2..fe139576a 100644 --- a/fuse_core/src/uuid.cpp +++ b/fuse_core/src/uuid.cpp @@ -61,7 +61,7 @@ UUID generate() return uuid; } -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp) { const auto nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds); @@ -69,7 +69,8 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) // Explicitly pack nanosecond bits from LSB -> MSB by masking and shifting // E.g. 10 would be 0x00000000'0000000A, so the buffer would store 0x0A at buffer[0] - for (size_t i = 0; i < sizeof(nanoseconds); i++) { + for (size_t i = 0; i < sizeof(nanoseconds); i++) + { auto mask = (nanoseconds & (static_cast(0xFF) << 8 * i)); buffer[i] = static_cast(mask >> 8 * i); } @@ -77,14 +78,15 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, const UUID & id) +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id) { const auto nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds) + UUID::static_size(); std::array buffer; // Explicitly pack nanosecond bits from LSB -> MSB by masking and shifting - for (size_t i = 0; i < sizeof(nanoseconds); i++) { + for (size_t i = 0; i < sizeof(nanoseconds); i++) + { auto mask = (nanoseconds & (static_cast(0xFF) << 8 * i)); buffer[i] = static_cast(mask >> 8 * i); } @@ -96,11 +98,9 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string & namespace_string, const uint64_t & user_id) +UUID generate(const std::string& namespace_string, const uint64_t& user_id) { - return generate( - namespace_string, reinterpret_cast(&user_id), - sizeof(user_id)); + return generate(namespace_string, reinterpret_cast(&user_id), sizeof(user_id)); } } // namespace uuid diff --git a/fuse_core/src/variable.cpp b/fuse_core/src/variable.cpp index 00b098ccb..6b8f40749 100644 --- a/fuse_core/src/variable.cpp +++ b/fuse_core/src/variable.cpp @@ -38,12 +38,11 @@ namespace fuse_core { -Variable::Variable(const UUID & uuid) -: uuid_(uuid) +Variable::Variable(const UUID& uuid) : uuid_(uuid) { } -std::ostream & operator<<(std::ostream & stream, const Variable & variable) +std::ostream& operator<<(std::ostream& stream, const Variable& variable) { variable.print(stream); return stream; diff --git a/fuse_core/test/example_constraint.hpp b/fuse_core/test/example_constraint.hpp index 4185a7784..4df0ef035 100644 --- a/fuse_core/test/example_constraint.hpp +++ b/fuse_core/test/example_constraint.hpp @@ -56,25 +56,24 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint( - const std::string & source, - std::initializer_list variable_uuid_list) - : fuse_core::Constraint(source, variable_uuid_list), - data(0.0) + ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } - template - ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), - data(0.0) + template + ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override {return nullptr;} + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override + { + return nullptr; + } double data; // Public member variable just for testing @@ -89,11 +88,11 @@ class ExampleConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_core/test/example_loss.hpp b/fuse_core/test/example_loss.hpp index 1421d1fca..2f8c11533 100644 --- a/fuse_core/test/example_loss.hpp +++ b/fuse_core/test/example_loss.hpp @@ -52,27 +52,27 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) - : a(a) + explicit ExampleLoss(const double a = 1.0) : a(a) { } void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override {} + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::LossFunction * lossFunction() const override + ceres::LossFunction* lossFunction() const override { return new ceres::HuberLoss(a); } - double a{1.0}; //!< Public member variable just for testing + double a{ 1.0 }; //!< Public member variable just for testing private: // Allow Boost Serialization access to private methods @@ -85,11 +85,11 @@ class ExampleLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a; + archive& boost::serialization::base_object(*this); + archive& a; } }; diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index 95ea87a6e..a0e6b80c0 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -53,16 +53,25 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - ExampleVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_(0.0) + ExampleVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) { } - size_t size() const override {return 1;} - const double * data() const override {return &data_;} - double * data() override {return &data_;} - void print(std::ostream & /*stream = std::cout*/) const override {} + size_t size() const override + { + return 1; + } + const double* data() const override + { + return &data_; + } + double* data() override + { + return &data_; + } + void print(std::ostream& /*stream = std::cout*/) const override + { + } #if CERES_SUPPORTS_MANIFOLDS /** @@ -70,7 +79,10 @@ class ExampleVariable : public fuse_core::Variable * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -86,11 +98,11 @@ class ExampleVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -102,11 +114,17 @@ class LegacyParameterization : public fuse_core::LocalParameterization public: FUSE_SMART_PTR_DEFINITIONS(LegacyParameterization) - int GlobalSize() const override {return 4;} + int GlobalSize() const override + { + return 4; + } - int LocalSize() const override {return 3;} + int LocalSize() const override + { + return 3; + } - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -114,22 +132,30 @@ class LegacyParameterization : public fuse_core::LocalParameterization 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; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus(const double * x, const double * y, double * y_minus_x) const override + bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; x_inverse[0] = x[0]; @@ -143,16 +169,25 @@ class LegacyParameterization : public fuse_core::LocalParameterization 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; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -168,10 +203,10 @@ class LegacyParameterization : public fuse_core::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 - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -180,16 +215,25 @@ 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} + 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 {} + 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. @@ -197,14 +241,17 @@ class LegacyVariable : public fuse_core::Variable * 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;} + 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 + fuse_core::LocalParameterization* localParameterization() const override { return new LegacyParameterization(); } @@ -221,11 +268,11 @@ class LegacyVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index 36da20160..17197b019 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -46,16 +46,14 @@ class TestParameters : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -68,34 +66,34 @@ class TestParameters : public ::testing::Test TEST_F(TestParameters, getPositiveParam) { // Load parameters enforcing they are positive: - const double default_value{1.0}; + const double default_value{ 1.0 }; auto node = rclcpp::Node::make_shared("test_parameters_node"); // Load a positive parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "positive_parameter", parameter); EXPECT_EQ(3.0, parameter); } // Load a negative parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "negative_parameter", parameter); EXPECT_EQ(default_value, parameter); } // Load a zero parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "zero_parameter", parameter); EXPECT_EQ(default_value, parameter); } // Load a zero parameter allowing zero (not strict): { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "zero_parameter", parameter, false); EXPECT_EQ(0.0, parameter); } @@ -119,91 +117,91 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) // Load covariance matrix diagonal from the parameter server: // A covariance diagonal with the expected size and valid should be the same as the expected one: { - const std::string parameter_name{"covariance_diagonal"}; + const std::string parameter_name{ "covariance_diagonal" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - try { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + try + { + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); EXPECT_EQ(Size, covariance.rows()); EXPECT_EQ(Size, covariance.cols()); - EXPECT_EQ( - expected_covariance.rows() * expected_covariance.cols(), - expected_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << expected_covariance << "\nActual\n" << covariance; - } catch (const std::exception & ex) { + EXPECT_EQ(expected_covariance.rows() * expected_covariance.cols(), + expected_covariance.cwiseEqual(covariance).count()) + << "Expected\n" + << expected_covariance << "\nActual\n" + << covariance; + } + catch (const std::exception& ex) + { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } } // If the parameter does not exist we should get the default covariance: { - const std::string parameter_name{"non_existent_parameter"}; + const std::string parameter_name{ "non_existent_parameter" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - try { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + try + { + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); EXPECT_EQ(Size, covariance.rows()); EXPECT_EQ(Size, covariance.cols()); - EXPECT_EQ( - default_covariance.rows() * default_covariance.cols(), - default_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << default_covariance << "\nActual\n" << covariance; - } catch (const std::exception & ex) { + EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), default_covariance.cwiseEqual(covariance).count()) + << "Expected\n" + << default_covariance << "\nActual\n" + << covariance; + } + catch (const std::exception& ex) + { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } } // A covariance diagonal with negative values should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_negative_values"}; + const std::string parameter_name{ "covariance_diagonal_with_negative_values" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } // A covariance diagonal with size 2, smaller than expected, should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_size_2"}; + const std::string parameter_name{ "covariance_diagonal_with_size_2" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } // A covariance diagonal with size 4, larger than expected, should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_size_4"}; + const std::string parameter_name{ "covariance_diagonal_with_size_4" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } - // A covariance diagonal with an invalid element should throw // rclcpp::exceptions::InvalidParameterTypeException: { - const std::string parameter_name{"covariance_diagonal_with_strings"}; + const std::string parameter_name{ "covariance_diagonal_with_strings" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - rclcpp::exceptions::InvalidParameterTypeException); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to // instead get substituted with default covariance. But now with strongly typed @@ -216,12 +214,11 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) // A covariance diagonal with an invalid element should throw // rclcpp::exceptions::InvalidParameterTypeException: { - const std::string parameter_name{"covariance_diagonal_with_string"}; + const std::string parameter_name{ "covariance_diagonal_with_string" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - rclcpp::exceptions::InvalidParameterTypeException); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to // instead get substituted with default covariance. But now with strongly typed @@ -232,9 +229,8 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) } } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_core/test/test_async_motion_model.cpp b/fuse_core/test/test_async_motion_model.cpp index 3e60abc15..3fb601983 100644 --- a/fuse_core/test/test_async_motion_model.cpp +++ b/fuse_core/test/test_async_motion_model.cpp @@ -42,15 +42,13 @@ class MyMotionModel : public fuse_core::AsyncMotionModel { public: - MyMotionModel() - : fuse_core::AsyncMotionModel(1), - initialized(false) + MyMotionModel() : fuse_core::AsyncMotionModel(1), initialized(false) { } virtual ~MyMotionModel() = default; - bool applyCallback(fuse_core::Transaction & /*transaction*/) + bool applyCallback(fuse_core::Transaction& /*transaction*/) { rclcpp::sleep_for(std::chrono::milliseconds(1000)); transaction_received = true; @@ -89,7 +87,8 @@ class TestAsyncMotionModel : public ::testing::Test TEST_F(TestAsyncMotionModel, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { MyMotionModel motion_model; auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); motion_model.initialize(*node, "my_motion_model_" + std::to_string(i)); @@ -120,13 +119,15 @@ TEST_F(TestAsyncMotionModel, OnGraphUpdate) auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of graphCallback to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { motion_model.graph_received = false; motion_model.graphCallback(graph); EXPECT_FALSE(motion_model.graph_received); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!motion_model.graph_received && clock.now() < wait_time_elapsed) { + while (!motion_model.graph_received && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(motion_model.graph_received); diff --git a/fuse_core/test/test_async_publisher.cpp b/fuse_core/test/test_async_publisher.cpp index c5dc42b34..fb3cf3e20 100644 --- a/fuse_core/test/test_async_publisher.cpp +++ b/fuse_core/test/test_async_publisher.cpp @@ -44,18 +44,13 @@ class MyPublisher : public fuse_core::AsyncPublisher { public: - MyPublisher() - : fuse_core::AsyncPublisher(1), - callback_processed(false), - initialized(false) + MyPublisher() : fuse_core::AsyncPublisher(1), callback_processed(false), initialized(false) { } virtual ~MyPublisher() = default; - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr /*transaction*/, - fuse_core::Graph::ConstSharedPtr /*graph*/) + void notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, fuse_core::Graph::ConstSharedPtr /*graph*/) { rclcpp::sleep_for(std::chrono::milliseconds(10)); callback_processed = true; @@ -86,7 +81,8 @@ class TestAsyncPublisher : public ::testing::Test TEST_F(TestAsyncPublisher, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { auto node = rclcpp::Node::make_shared("test_async_pub_node"); MyPublisher publisher; publisher.initialize(*node, "my_publisher_" + std::to_string(i)); @@ -114,17 +110,19 @@ TEST_F(TestAsyncPublisher, notifyCallback) // MyPublisher's async spinner. There is a time delay there. So, this call should return almost // immediately, then we have to wait a bit before the "callback_processed" flag gets flipped. fuse_core::Transaction::ConstSharedPtr transaction; // nullptr is ok as we don't actually use it - fuse_core::Graph::ConstSharedPtr graph; // nullptr is ok as we don't actually use it + fuse_core::Graph::ConstSharedPtr graph; // nullptr is ok as we don't actually use it auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of notify to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { publisher.callback_processed = false; publisher.notify(transaction, graph); EXPECT_FALSE(publisher.callback_processed); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!publisher.callback_processed && clock.now() < wait_time_elapsed) { + while (!publisher.callback_processed && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(publisher.callback_processed); diff --git a/fuse_core/test/test_async_sensor_model.cpp b/fuse_core/test/test_async_sensor_model.cpp index f19d9c5fe..52b70cbf6 100644 --- a/fuse_core/test/test_async_sensor_model.cpp +++ b/fuse_core/test/test_async_sensor_model.cpp @@ -56,9 +56,7 @@ void transactionCallback(fuse_core::Transaction::SharedPtr /*transaction*/) class MySensor : public fuse_core::AsyncSensorModel { public: - MySensor() - : fuse_core::AsyncSensorModel(1), - initialized(false) + MySensor() : fuse_core::AsyncSensorModel(1), initialized(false) { } @@ -95,7 +93,8 @@ class TestAsyncSensorModel : public ::testing::Test TEST_F(TestAsyncSensorModel, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { MySensor sensor; auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); sensor.initialize(*node, "my_sensor_" + std::to_string(i), &transactionCallback); @@ -126,12 +125,14 @@ TEST_F(TestAsyncSensorModel, OnGraphUpdate) auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of graphCallback to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { sensor.graph_received = false; sensor.graphCallback(graph); EXPECT_FALSE(sensor.graph_received); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!sensor.graph_received && clock.now() < wait_time_elapsed) { + while (!sensor.graph_received && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(sensor.graph_received); diff --git a/fuse_core/test/test_callback_wrapper.cpp b/fuse_core/test/test_callback_wrapper.cpp index 9bc4932d3..502ff2491 100644 --- a/fuse_core/test/test_callback_wrapper.cpp +++ b/fuse_core/test/test_callback_wrapper.cpp @@ -44,12 +44,12 @@ class MyClass { public: - double processData(const std::vector & data) + double processData(const std::vector& data) { return std::accumulate(data.begin(), data.end(), 0.0); } - void processDataInPlace(const std::vector & data, double & output) + void processDataInPlace(const std::vector& data, double& output) { output = std::accumulate(data.begin(), data.end(), 0.0); } @@ -72,16 +72,14 @@ class TestCallbackWrapper : public ::testing::Test TEST_F(TestCallbackWrapper, Double) { MyClass my_object; - std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; + std::vector data = { 1.0, 2.0, 3.0, 4.0, 5.0 }; auto node = rclcpp::Node::make_shared("callback_wrapper_double_test_node"); - auto callback_queue = - std::make_shared(node->get_node_base_interface()->get_context()); - node->get_node_waitables_interface()->add_waitable( - callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + auto callback_queue = std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable(callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); auto callback = std::make_shared>( - std::bind(&MyClass::processData, &my_object, std::ref(data))); + std::bind(&MyClass::processData, &my_object, std::ref(data))); auto result = callback->getFuture(); callback_queue->addCallback(callback); @@ -95,17 +93,15 @@ TEST_F(TestCallbackWrapper, Double) TEST_F(TestCallbackWrapper, Void) { MyClass my_object; - std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; + std::vector data = { 1.0, 2.0, 3.0, 4.0, 5.0 }; double output; auto node = rclcpp::Node::make_shared("callback_wrapper_void_test_node"); - auto callback_queue = - std::make_shared(node->get_node_base_interface()->get_context()); - node->get_node_waitables_interface()->add_waitable( - callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + auto callback_queue = std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable(callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); auto callback = std::make_shared>( - std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output))); + std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output))); auto result = callback->getFuture(); callback_queue->addCallback(callback); diff --git a/fuse_core/test/test_constraint.cpp b/fuse_core/test/test_constraint.cpp index b278c2153..39e6c4cc8 100644 --- a/fuse_core/test/test_constraint.cpp +++ b/fuse_core/test/test_constraint.cpp @@ -43,7 +43,7 @@ TEST(Constraint, Constructor) // Create a constraint with a single UUID { fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1 }); // NOLINT ASSERT_EQ(1u, constraint.variables().size()); ASSERT_EQ(variable_uuid1, constraint.variables().at(0)); } @@ -52,7 +52,7 @@ TEST(Constraint, Constructor) fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid2 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid3 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1, variable_uuid2, variable_uuid3}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1, variable_uuid2, variable_uuid3 }); // NOLINT ASSERT_EQ(3u, constraint.variables().size()); ASSERT_EQ(variable_uuid1, constraint.variables().at(0)); ASSERT_EQ(variable_uuid2, constraint.variables().at(1)); @@ -67,7 +67,8 @@ TEST(Constraint, Constructor) variable_uuids.push_back(fuse_core::uuid::generate()); ExampleConstraint constraint("test", variable_uuids.begin(), variable_uuids.end()); ASSERT_EQ(variable_uuids.size(), constraint.variables().size()); - for (size_t i = 0; i < variable_uuids.size(); ++i) { + for (size_t i = 0; i < variable_uuids.size(); ++i) + { ASSERT_EQ(variable_uuids.at(i), constraint.variables().at(i)); } } @@ -76,12 +77,13 @@ TEST(Constraint, Constructor) fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid2 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid3 = fuse_core::uuid::generate(); - ExampleConstraint constraint1("test", {variable_uuid1, variable_uuid2, variable_uuid3}); // NOLINT + ExampleConstraint constraint1("test", { variable_uuid1, variable_uuid2, variable_uuid3 }); // NOLINT ExampleConstraint constraint2(constraint1); ASSERT_EQ(constraint1.uuid(), constraint2.uuid()); ASSERT_EQ(constraint1.variables().size(), constraint2.variables().size()); - for (size_t i = 0; i < constraint1.variables().size(); ++i) { + for (size_t i = 0; i < constraint1.variables().size(); ++i) + { ASSERT_EQ(constraint1.variables().at(i), constraint2.variables().at(i)); } } @@ -90,6 +92,6 @@ TEST(Constraint, Constructor) TEST(Constraint, Type) { fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1 }); // NOLINT ASSERT_EQ("ExampleConstraint", constraint.type()); } diff --git a/fuse_core/test/test_eigen.cpp b/fuse_core/test/test_eigen.cpp index c7b2ff6db..f28c9679a 100644 --- a/fuse_core/test/test_eigen.cpp +++ b/fuse_core/test/test_eigen.cpp @@ -43,8 +43,7 @@ TEST(Eigen, isSymmetric) const auto symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); EXPECT_TRUE(fuse_core::isSymmetric(symmetric_matrix)) << "Matrix\n" - << symmetric_matrix << - "\n expected to be symmetric."; + << symmetric_matrix << "\n expected to be symmetric."; // A non-symmetric matrix: const double asymmetry_error = 1.0e-6; @@ -53,15 +52,15 @@ TEST(Eigen, isSymmetric) non_symmetric_matrix(0, 1) += asymmetry_error; EXPECT_FALSE(fuse_core::isSymmetric(non_symmetric_matrix)) - << "Matrix\n" - << non_symmetric_matrix << "\n expected to not be symmetric."; + << "Matrix\n" + << non_symmetric_matrix << "\n expected to not be symmetric."; // Checking symmetry with precision larger than asymmetry error in non-symmetric matrix: const double precision = 1.0e2 * asymmetry_error; EXPECT_TRUE(fuse_core::isSymmetric(non_symmetric_matrix, precision)) - << "Matrix\n" - << non_symmetric_matrix << "\n expected to be symmetric with precision " << precision << "."; + << "Matrix\n" + << non_symmetric_matrix << "\n expected to be symmetric with precision " << precision << "."; // fuse_core::isSymmetric is not defined for non-square matrices. The following will simply fail // to compile because it is not allowed, as intended: @@ -80,16 +79,15 @@ TEST(Eigen, isPositiveDefinite) const auto psd_matrix = (symmetric_matrix + 3 * fuse_core::Matrix3d::Identity()).eval(); EXPECT_TRUE(fuse_core::isPositiveDefinite(psd_matrix)) << "Matrix\n" - << psd_matrix << - "\n expected to be Positive Definite."; + << psd_matrix << "\n expected to be Positive Definite."; // A non Positive Definite matrix: auto non_psd_matrix = psd_matrix; non_psd_matrix(0, 0) *= -1.0; EXPECT_FALSE(fuse_core::isPositiveDefinite(non_psd_matrix)) - << "Matrix\n" - << non_psd_matrix << "\n expected to not be Positive Definite."; + << "Matrix\n" + << non_psd_matrix << "\n expected to not be Positive Definite."; // fuse_core::isPositiveDefinite is not defined for non-square matrices. The following will simply // fail to compile because it is allowed, as intended: diff --git a/fuse_core/test/test_local_parameterization.cpp b/fuse_core/test/test_local_parameterization.cpp index 29273ed58..bf5c7185e 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -42,8 +42,8 @@ struct Plus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = x[0] + 2.0 * delta[0]; x_plus_delta[1] = x[1] + 5.0 * delta[1]; @@ -54,8 +54,8 @@ struct Plus struct Minus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { y_minus_x[0] = (y[0] - x[0]) / 2.0; y_minus_x[1] = (y[1] - x[1]) / 5.0; @@ -65,14 +65,13 @@ 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,16 +84,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); /* *INDENT-OFF* */ // Bypass uncrustify - 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; /* *INDENT-ON* */ EXPECT_TRUE(success); @@ -105,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); @@ -119,15 +116,14 @@ 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); /* *INDENT-OFF* */ // Bypass uncrustify - 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; /* *INDENT-ON* */ EXPECT_TRUE(success); @@ -138,8 +134,8 @@ TEST(LocalParameterization, MinusSameVariablesIsZero) { TestLocalParameterization parameterization; - double x1[3] = {1.0, 2.0, 3.0}; - double actual[2] = {0.0, 0.0}; + double x1[3] = { 1.0, 2.0, 3.0 }; + double actual[2] = { 0.0, 0.0 }; bool success = parameterization.Minus(x1, x1, actual); EXPECT_TRUE(success); @@ -151,14 +147,14 @@ TEST(LocalParameterization, PlusMinus) { TestLocalParameterization parameterization; - const double x1[3] = {1.0, 2.0, 3.0}; - const double delta[2] = {0.5, 1.0}; - double x2[3] = {0.0, 0.0, 0.0}; + const double x1[3] = { 1.0, 2.0, 3.0 }; + const double delta[2] = { 0.5, 1.0 }; + double x2[3] = { 0.0, 0.0, 0.0 }; bool success = parameterization.Plus(x1, delta, x2); ASSERT_TRUE(success); - double actual[2] = {0.0, 0.0}; + double actual[2] = { 0.0, 0.0 }; success = parameterization.Minus(x1, x2, actual); EXPECT_TRUE(success); diff --git a/fuse_core/test/test_loss.cpp b/fuse_core/test/test_loss.cpp index 7992979d0..4628836ee 100644 --- a/fuse_core/test/test_loss.cpp +++ b/fuse_core/test/test_loss.cpp @@ -37,7 +37,7 @@ TEST(Loss, Constructor) { - const double a{0.3}; + const double a{ 0.3 }; ExampleLoss loss(a); ASSERT_EQ(a, loss.a); @@ -45,7 +45,8 @@ TEST(Loss, Constructor) ASSERT_NE(nullptr, loss_function); - if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { + if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) + { delete loss_function; } } diff --git a/fuse_core/test/test_message_buffer.cpp b/fuse_core/test/test_message_buffer.cpp index 33ab74e77..65a430e3a 100644 --- a/fuse_core/test/test_message_buffer.cpp +++ b/fuse_core/test/test_message_buffer.cpp @@ -43,8 +43,7 @@ class MessageBufferTestFixture : public ::testing::Test { public: - MessageBufferTestFixture() - : buffer(rclcpp::Duration::max()) + MessageBufferTestFixture() : buffer(rclcpp::Duration::max()) { } @@ -63,9 +62,7 @@ class MessageBufferTestFixture : public ::testing::Test TEST_F(MessageBufferTestFixture, Exceptions) { // Call the query with the parameters in the wrong order. This should throw. - EXPECT_THROW( - buffer.query(rclcpp::Time(20, 0), rclcpp::Time(10, 0), false), - std::invalid_argument); + EXPECT_THROW(buffer.query(rclcpp::Time(20, 0), rclcpp::Time(10, 0), false), std::invalid_argument); // Call the query when the buffer is empty. This should throw. EXPECT_THROW(buffer.query(rclcpp::Time(10, 0), rclcpp::Time(25, 0), false), std::out_of_range); diff --git a/fuse_core/test/test_parameter.cpp b/fuse_core/test/test_parameter.cpp index c1af54cd0..3013c89fe 100644 --- a/fuse_core/test/test_parameter.cpp +++ b/fuse_core/test/test_parameter.cpp @@ -42,53 +42,41 @@ TEST(parameter, list_parameter_override_prefixes) { const std::map overrides = { - {"foo", {}}, - {"foo.baz", {}}, - {"foo.bar", {}}, - {"foo.bar.baz", {}}, - {"foo.bar.baz.bing", {}}, - {"foobar.baz", {}}, - {"baz", {}} + { "foo", {} }, { "foo.baz", {} }, { "foo.bar", {} }, { "foo.bar.baz", {} }, { "foo.bar.baz.bing", {} }, + { "foobar.baz", {} }, { "baz", {} } }; { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo"); EXPECT_EQ(2u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.baz")); EXPECT_NE(matches.end(), matches.find("foo.bar")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.bar"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.bar"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.bar.baz")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.bar.baz"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.bar.baz"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.bar.baz.bing")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.baz"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.baz"); EXPECT_EQ(0u, matches.size()); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foobar"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foobar"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foobar.baz")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "done"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "done"); EXPECT_EQ(0u, matches.size()); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, ""); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, ""); EXPECT_EQ(3u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo")); EXPECT_NE(matches.end(), matches.find("foobar")); diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index da1ccefa3..844e9472a 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -52,9 +52,7 @@ class PointPublisher : public rclcpp::Node * * @param[in] frequency The publishing frequency in Hz */ - explicit PointPublisher(const double frequency) - : Node("point_publisher_node") - , frequency_(frequency) + explicit PointPublisher(const double frequency) : Node("point_publisher_node"), frequency_(frequency) { publisher_ = this->create_publisher("point", 1); } @@ -78,7 +76,8 @@ class PointPublisher : public rclcpp::Node { // Wait for the subscriptions to be ready before sending them data: rclcpp::Time subscription_timeout = this->now() + rclcpp::Duration::from_seconds(1.0); - while (publisher_->get_subscription_count() < 1u && this->now() < subscription_timeout) { + while (publisher_->get_subscription_count() < 1u && this->now() < subscription_timeout) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } @@ -86,7 +85,8 @@ class PointPublisher : public rclcpp::Node // Send data: rclcpp::Rate rate(frequency_); - for (size_t i = 0; i < num_messages; ++i) { + for (size_t i = 0; i < num_messages; ++i) + { geometry_msgs::msg::Point point_message; point_message.x = i; publisher_->publish(point_message); @@ -96,7 +96,7 @@ class PointPublisher : public rclcpp::Node private: rclcpp::Publisher::SharedPtr publisher_; //!< The publisher - double frequency_{10.0}; //!< The publish rate frequency + double frequency_{ 10.0 }; //!< The publish rate frequency }; /** @@ -115,20 +115,15 @@ class PointSensorModel : public rclcpp::Node * * @param[in] throttle_period The throttle period duration in seconds */ - explicit PointSensorModel(const rclcpp::Duration & throttle_period) - : Node("point_sensor_model_node") - , throttled_callback_( - std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), - std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), - throttle_period - ) + explicit PointSensorModel(const rclcpp::Duration& throttle_period) + : Node("point_sensor_model_node") + , throttled_callback_(std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), + std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), throttle_period) { subscription_ = this->create_subscription( - "point", 10, - std::bind( - &PointThrottledCallback::callback, - &throttled_callback_, std::placeholders::_1) - ); + "point", 10, + std::bind(&PointThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1)); } /** @@ -178,7 +173,7 @@ class PointSensorModel : public rclcpp::Node * * @param[in] msg A geometry_msgs::msg::Point message */ - void keepCallback(const geometry_msgs::msg::Point & msg) + void keepCallback(const geometry_msgs::msg::Point& msg) { ++kept_messages_; last_kept_message_ = std::make_shared(msg); @@ -190,18 +185,18 @@ class PointSensorModel : public rclcpp::Node * @param[in] msg A geometry_msgs::msg::Point message (not used) */ // NOTE(CH3): The msg arg here is necessary to allow binding the throttled callback - void dropCallback(const geometry_msgs::msg::Point & /*msg*/) + void dropCallback(const geometry_msgs::msg::Point& /*msg*/) { ++dropped_messages_; } - rclcpp::Subscription::SharedPtr subscription_; //!< The subscription + rclcpp::Subscription::SharedPtr subscription_; //!< The subscription using PointThrottledCallback = fuse_core::ThrottledMessageCallback; PointThrottledCallback throttled_callback_; //!< The throttled callback - size_t kept_messages_{0}; //!< Messages kept - size_t dropped_messages_{0}; //!< Messages dropped + size_t kept_messages_{ 0 }; //!< Messages kept + size_t dropped_messages_{ 0 }; //!< Messages dropped // We use a SharedPtr to check for nullptr just for this test geometry_msgs::msg::Point::SharedPtr last_kept_message_; //!< The last message kept @@ -214,17 +209,15 @@ class TestThrottledCallback : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); rclcpp::shutdown(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -242,9 +235,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); // Publish some messages: const size_t num_messages = 10; @@ -268,9 +259,7 @@ TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPe executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); // Publish some messages at half the throttled period: const size_t num_messages = 10; @@ -298,9 +287,7 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); ASSERT_EQ(nullptr, sensor_model->getLastKeptMessage()); diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index 125ff1eeb..b5a7a8530 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -52,13 +52,9 @@ class TimestampManagerTestFixture : public ::testing::Test { public: TimestampManagerTestFixture() - : manager(std::bind(&TimestampManagerTestFixture::generator, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - rclcpp::Duration::max()) + : manager(std::bind(&TimestampManagerTestFixture::generator, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + rclcpp::Duration::max()) { } @@ -74,11 +70,9 @@ class TimestampManagerTestFixture : public ::testing::Test generated_time_spans.clear(); } - void generator( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & /*constraints*/, - std::vector & /*variables*/) + void generator(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& /*constraints*/, + std::vector& /*variables*/) { generated_time_spans.emplace_back(beginning_stamp, ending_stamp); } @@ -87,7 +81,6 @@ class TimestampManagerTestFixture : public ::testing::Test std::vector> generated_time_spans; }; - TEST_F(TimestampManagerTestFixture, Empty) { // Test: diff --git a/fuse_core/test/test_transaction.cpp b/fuse_core/test/test_transaction.cpp index 159acd848..7f60fa279 100644 --- a/fuse_core/test/test_transaction.cpp +++ b/fuse_core/test/test_transaction.cpp @@ -46,7 +46,6 @@ using fuse_core::Transaction; using fuse_core::UUID; - /** * @brief Test that a collection of stamps exist in a Transaction object's involved stamps container * @@ -59,29 +58,31 @@ using fuse_core::UUID; * @return True if the expected stamps, and only the expected stamps, exist in the * transaction, False otherwise. */ -template -bool testInvolvedStamps(const TimeRange & expected, const Transaction & transaction) +template +bool testInvolvedStamps(const TimeRange& expected, const Transaction& transaction) { auto range = transaction.involvedStamps(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_stamp = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_stamp = *iter; bool found = false; - for (const auto & expected_stamp : expected) { - if (actual_stamp == expected_stamp) { + for (const auto& expected_stamp : expected) + { + if (actual_stamp == expected_stamp) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -103,43 +104,44 @@ bool testInvolvedStamps(const TimeRange & expected, const Transaction & transact * @return True if the expected constraints, and only the expected constraints, * exist in the transaction, False otherwise. */ -template -bool testAddedConstraints(const ConstraintRange & expected, const Transaction & transaction) +template +bool testAddedConstraints(const ConstraintRange& expected, const Transaction& transaction) { auto range = transaction.addedConstraints(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_constraint = dynamic_cast(*iter); + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_constraint = dynamic_cast(*iter); bool found = false; - for (const auto & expected_constraint : expected) { - if (actual_constraint.uuid() == expected_constraint.uuid()) { + for (const auto& expected_constraint : expected) + { + if (actual_constraint.uuid() == expected_constraint.uuid()) + { found = true; bool is_equal = true; is_equal = is_equal && (expected_constraint.type() == actual_constraint.type()); - is_equal = is_equal && - (expected_constraint.variables().size() == actual_constraint.variables().size()); - for (size_t i = 0; i < expected_constraint.variables().size(); ++i) { - is_equal = is_equal && - (expected_constraint.variables().at(i) == actual_constraint.variables().at(i)); + is_equal = is_equal && (expected_constraint.variables().size() == actual_constraint.variables().size()); + for (size_t i = 0; i < expected_constraint.variables().size(); ++i) + { + is_equal = is_equal && (expected_constraint.variables().at(i) == actual_constraint.variables().at(i)); } - const auto & expected_derived = - dynamic_cast(expected_constraint); + const auto& expected_derived = dynamic_cast(expected_constraint); is_equal = is_equal && (expected_derived.data == actual_constraint.data); - if (!is_equal) { + if (!is_equal) + { return false; } } } - if (!found) { + if (!found) + { return false; } } @@ -162,29 +164,31 @@ bool testAddedConstraints(const ConstraintRange & expected, const Transaction & * @return True if the expected constraints, and only the expected constraints, exist in * the transaction, False otherwise. */ -template -bool testRemovedConstraints(const UuidRange & expected, const Transaction & transaction) +template +bool testRemovedConstraints(const UuidRange& expected, const Transaction& transaction) { auto range = transaction.removedConstraints(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_constraint_uuid = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_constraint_uuid = *iter; bool found = false; - for (const auto & expected_constraint_uuid : expected) { - if (actual_constraint_uuid == expected_constraint_uuid) { + for (const auto& expected_constraint_uuid : expected) + { + if (actual_constraint_uuid == expected_constraint_uuid) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -205,36 +209,39 @@ bool testRemovedConstraints(const UuidRange & expected, const Transaction & tran * @return True if the expected variables, and only the expected variables, exist in * the transaction, False otherwise. */ -template -bool testAddedVariables(const VariableRange & expected, const Transaction & transaction) +template +bool testAddedVariables(const VariableRange& expected, const Transaction& transaction) { auto range = transaction.addedVariables(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_variable = dynamic_cast(*iter); + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_variable = dynamic_cast(*iter); bool found = false; - for (const auto & expected_variable : expected) { - if (actual_variable.uuid() == expected_variable.uuid()) { + for (const auto& expected_variable : expected) + { + if (actual_variable.uuid() == expected_variable.uuid()) + { found = true; bool is_equal = true; is_equal = is_equal && (expected_variable.type() == actual_variable.type()); is_equal = is_equal && (expected_variable.size() == actual_variable.size()); is_equal = is_equal && (expected_variable.data()[0] == actual_variable.data()[0]); - if (!is_equal) { + if (!is_equal) + { return false; } } } - if (!found) { + if (!found) + { return false; } } @@ -256,29 +263,31 @@ bool testAddedVariables(const VariableRange & expected, const Transaction & tran * @return True if the expected variables, and only the expected variables, exist in the * transaction, False otherwise. */ -template -bool testRemovedVariables(const UuidRange & expected, const Transaction & transaction) +template +bool testRemovedVariables(const UuidRange& expected, const Transaction& transaction) { auto range = transaction.removedVariables(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_variable_uuid = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_variable_uuid = *iter; bool found = false; - for (const auto & expected_variable_uuid : expected) { - if (actual_variable_uuid == expected_variable_uuid) { + for (const auto& expected_variable_uuid : expected) + { + if (actual_variable_uuid == expected_variable_uuid) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -299,8 +308,7 @@ TEST(Transaction, Empty) // A transaction with added constraints cannot be empty { const auto variable_uuid = fuse_core::uuid::generate(); - const auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + const auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -354,8 +362,7 @@ TEST(Transaction, AddConstraint) // Add a single constraint and verify it exists in the added constraints { UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -368,8 +375,7 @@ TEST(Transaction, AddConstraint) // Add the same constraint multiple times. Verify only one constraint exists in the Transaction. { UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -384,14 +390,11 @@ TEST(Transaction, AddConstraint) // Add multiple constraints. Verify they all exist in the Transaction. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID variable2_uuid = fuse_core::uuid::generate(); - auto constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID variable3_uuid = fuse_core::uuid::generate(); - auto constraint3 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable3_uuid}); + auto constraint3 = ExampleConstraint::make_shared("test", std::initializer_list{ variable3_uuid }); Transaction transaction; transaction.addConstraint(constraint1); @@ -409,8 +412,7 @@ TEST(Transaction, AddConstraint) // also be deleted from the 'removed' container. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID constraint2_uuid = fuse_core::uuid::generate(); @@ -430,8 +432,7 @@ TEST(Transaction, AddConstraint) { // Create and add the constraint to the transaction UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); constraint1->data = 1.0; Transaction transaction; @@ -521,12 +522,10 @@ TEST(Transaction, RemoveConstraint) // marked for removal; instead it should be deleted from the added constraints. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID variable2_uuid = fuse_core::uuid::generate(); - auto constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); Transaction transaction; transaction.addConstraint(constraint1); @@ -729,12 +728,9 @@ TEST(Transaction, Merge) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); UUID variable3_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); - auto added_constraint3 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable3_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); + auto added_constraint3 = ExampleConstraint::make_shared("test", std::initializer_list{ variable3_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); @@ -818,10 +814,8 @@ TEST(Transaction, Clone) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); @@ -882,10 +876,8 @@ TEST(Transaction, Serialize) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); diff --git a/fuse_core/test/test_uuid.cpp b/fuse_core/test/test_uuid.cpp index 17041a8dd..5f6225c74 100644 --- a/fuse_core/test/test_uuid.cpp +++ b/fuse_core/test/test_uuid.cpp @@ -43,7 +43,6 @@ using fuse_core::UUID; using UUIDs = std::vector; - TEST(UUID, Generate) { // These tests are mostly just calling the different generate() signatures to verify they compile @@ -59,8 +58,7 @@ TEST(UUID, Generate) // UUID. { std::string buffer1 = "Curse your sudden but inevitable betrayal!"; - std::string buffer2 = - "Man walks down the street in a hat like that, you know he's not afraid of anything."; + std::string buffer2 = "Man walks down the street in a hat like that, you know he's not afraid of anything."; UUID id1 = fuse_core::uuid::generate(buffer1.data(), buffer1.size()); UUID id2 = fuse_core::uuid::generate(buffer1.data(), buffer1.size()); UUID id3 = fuse_core::uuid::generate(buffer2.data(), buffer2.size()); @@ -86,8 +84,7 @@ TEST(UUID, Generate) { std::string name1 = "Jayne"; std::string name2 = "Hoban"; - std::string buffer1 = - "Ten percent of nothing is, let me do the math here. Nothing into nothin'. Carry the nothin'"; + std::string buffer1 = "Ten percent of nothing is, let me do the math here. Nothing into nothin'. Carry the nothin'"; std::string buffer2 = "Some people juggle geese."; UUID id1 = fuse_core::uuid::generate(name1, buffer1.data(), buffer1.size()); @@ -167,11 +164,12 @@ TEST(UUID, Generate) } } -void generateUUIDs(UUIDs & uuids) +void generateUUIDs(UUIDs& uuids) { constexpr size_t uuid_count = 100000; uuids.reserve(uuid_count); - for (size_t i = 0; i < uuid_count; ++i) { + for (size_t i = 0; i < uuid_count; ++i) + { auto uuid = fuse_core::uuid::generate(); uuids.push_back(uuid); } @@ -185,10 +183,9 @@ TEST(UUID, CollisionSingleThread) // Check for duplicates std::unordered_set unique_uuids; - for (const auto & uuid : raw_uuids) { - ASSERT_TRUE( - unique_uuids.find(uuid) == - unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); + for (const auto& uuid : raw_uuids) + { + ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); } } @@ -199,20 +196,22 @@ TEST(UUID, CollisionManyThreads) constexpr size_t thread_count = 12; std::vector raw_uuids(thread_count); std::vector threads(thread_count); - for (size_t i = 0; i < threads.size(); ++i) { + for (size_t i = 0; i < threads.size(); ++i) + { threads[i] = std::thread(generateUUIDs, std::ref(raw_uuids[i])); } - for (size_t i = 0; i < threads.size(); ++i) { + for (size_t i = 0; i < threads.size(); ++i) + { threads[i].join(); } // Check for duplicates std::unordered_set unique_uuids; - for (size_t i = 0; i < raw_uuids.size(); ++i) { - for (const auto & uuid : raw_uuids[i]) { - ASSERT_TRUE( - unique_uuids.find(uuid) == - unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); + for (size_t i = 0; i < raw_uuids.size(); ++i) + { + for (const auto& uuid : raw_uuids[i]) + { + ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); } } diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index d4c8caa9e..4198913a6 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -43,12 +43,14 @@ #include "example_variable.hpp" -TEST(Variable, Type) { +TEST(Variable, Type) +{ ExampleVariable variable; ASSERT_EQ("ExampleVariable", variable.type()); } -TEST(LegacyVariable, Serialization) { +TEST(LegacyVariable, Serialization) +{ // Create an Orientation3DStamped LegacyVariable expected; expected.data()[0] = 0.952; @@ -80,7 +82,7 @@ TEST(LegacyVariable, Serialization) { #if CERES_SUPPORTS_MANIFOLDS struct QuaternionCostFunction { - explicit QuaternionCostFunction(double * observation) + explicit QuaternionCostFunction(double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -88,24 +90,12 @@ struct QuaternionCostFunction observation_[3] = observation[3]; } - template - bool operator()(const T * quaternion, T * residual) const + 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 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]; @@ -122,7 +112,8 @@ struct QuaternionCostFunction double observation_[4]; }; -TEST(LegacyVariable, ManifoldAdapter) { +TEST(LegacyVariable, ManifoldAdapter) +{ // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees LegacyVariable orientation; orientation.data()[0] = 0.952; @@ -131,19 +122,16 @@ TEST(LegacyVariable, ManifoldAdapter) { 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)); + 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; + 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::Summary summary; @@ -157,7 +145,8 @@ TEST(LegacyVariable, ManifoldAdapter) { EXPECT_NEAR(target_quat[3], orientation.data()[3], 1.0e-3); } -TEST(LegacyVariable, Deserialization) { +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. @@ -190,10 +179,10 @@ TEST(LegacyVariable, Deserialization) { // Test the manifold interface, and that the Legacy LocalParameterization is wrapped // in a ManifoldAdapter - fuse_core::Manifold * actual_manifold = nullptr; + fuse_core::Manifold* actual_manifold = nullptr; ASSERT_NO_THROW(actual_manifold = actual.manifold()); ASSERT_NE(actual_manifold, nullptr); - auto actual_manifold_adapter = dynamic_cast(actual_manifold); + auto actual_manifold_adapter = dynamic_cast(actual_manifold); ASSERT_NE(actual_manifold_adapter, nullptr); } #endif diff --git a/fuse_graphs/benchmark/benchmark_create_problem.cpp b/fuse_graphs/benchmark/benchmark_create_problem.cpp index a187e4a8e..eb1fdc7d5 100644 --- a/fuse_graphs/benchmark/benchmark_create_problem.cpp +++ b/fuse_graphs/benchmark/benchmark_create_problem.cpp @@ -63,15 +63,15 @@ class TestableHashGraph : public fuse_graphs::HashGraph class ExampleFunctor { public: - explicit ExampleFunctor(const std::vector & b) - : b_(b) + explicit ExampleFunctor(const std::vector& b) : b_(b) { } - template - bool operator()(T const * const * variables, T * residuals) const + template + bool operator()(T const* const* variables, T* residuals) const { - for (size_t i = 0; i < b_.size(); ++i) { + for (size_t i = 0; i < b_.size(); ++i) + { residuals[i] = variables[i][0] - T(b_[i]); } return true; @@ -91,22 +91,21 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - template - explicit ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), - data(std::distance(first, last), 0.0) + template + explicit ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(std::distance(first, last), 0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override + void print(std::ostream& /*stream = std::cout*/) const override { - auto cost_function = - new ceres::DynamicAutoDiffCostFunction(new ExampleFunctor(data)); + } + ceres::CostFunction* costFunction() const override + { + auto cost_function = new ceres::DynamicAutoDiffCostFunction(new ExampleFunctor(data)); - for (size_t i = 0; i < data.size(); ++i) { + for (size_t i = 0; i < data.size(); ++i) + { cost_function->AddParameterBlock(1); } @@ -128,11 +127,11 @@ class ExampleConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; @@ -145,52 +144,48 @@ BOOST_CLASS_EXPORT(ExampleConstraint); * @param[in] num_variables_per_constraint Number of variables the constraints should have * @return The TestableHashGraph */ -TestableHashGraph makeTestableHashGraph( - const size_t num_constraints, - const size_t num_variables_per_constraint) +TestableHashGraph makeTestableHashGraph(const size_t num_constraints, const size_t num_variables_per_constraint) { TestableHashGraph graph; - for (size_t i = 0; i < num_constraints; ++i) { + for (size_t i = 0; i < num_constraints; ++i) + { // Generate variables std::vector variables; variables.reserve(num_variables_per_constraint); - std::generate_n( - std::back_inserter(variables), num_variables_per_constraint, - []() {return ExampleVariable::make_shared();}); // NOLINT + std::generate_n(std::back_inserter(variables), num_variables_per_constraint, + []() { return ExampleVariable::make_shared(); }); // NOLINT // Add variables to the graph - for (const auto & variable : variables) { + for (const auto& variable : variables) + { graph.addVariable(variable); } // Add constraint with the generated variables std::vector variable_uuids; variable_uuids.reserve(variables.size()); - std::transform( - variables.begin(), variables.end(), std::back_inserter(variable_uuids), - [](const auto & variable) {return variable->uuid();}); // NOLINT - - graph.addConstraint( - ExampleConstraint::make_shared( - "test", variable_uuids.begin(), - variable_uuids.end())); + std::transform(variables.begin(), variables.end(), std::back_inserter(variable_uuids), + [](const auto& variable) { return variable->uuid(); }); // NOLINT + + graph.addConstraint(ExampleConstraint::make_shared("test", variable_uuids.begin(), variable_uuids.end())); } return graph; } -static void BM_createProblem(benchmark::State & state) +static void BM_createProblem(benchmark::State& state) { const auto graph = makeTestableHashGraph(state.range(0), state.range(1)); ceres::Problem problem; - for (auto _ : state) { + for (auto _ : state) + { graph.createProblem(problem); } } -BENCHMARK(BM_createProblem)->RangeMultiplier(2)->Ranges({{200, 4000}, {2, 12}}); // NOLINT +BENCHMARK(BM_createProblem)->RangeMultiplier(2)->Ranges({ { 200, 4000 }, { 2, 12 } }); // NOLINT BENCHMARK_MAIN(); diff --git a/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp b/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp index ec200fe87..7f9b80e29 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp @@ -39,7 +39,6 @@ #include #include - namespace fuse_graphs { @@ -61,10 +60,7 @@ struct HashGraphParams * * @param[in] interfaces - The node interfaces with which to load parameters */ - void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Parameters - > interfaces) + void loadFromROS(fuse_core::node_interfaces::NodeInterfaces interfaces) { fuse_core::loadProblemOptionsFromROS(interfaces, problem_options, "problem_options"); } diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 0f14bb7fc..14e385447 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -50,39 +50,30 @@ namespace fuse_graphs { -HashGraph::HashGraph(const HashGraphParams & params) -: problem_options_(params.problem_options) +HashGraph::HashGraph(const HashGraphParams& params) : problem_options_(params.problem_options) { // Set Ceres loss function ownership according to the fuse_core::Loss specification problem_options_.loss_function_ownership = fuse_core::Loss::Ownership; } -HashGraph::HashGraph(const HashGraph & other) -: constraints_by_variable_uuid_(other.constraints_by_variable_uuid_), - problem_options_(other.problem_options_), - variables_on_hold_(other.variables_on_hold_) +HashGraph::HashGraph(const HashGraph& other) + : constraints_by_variable_uuid_(other.constraints_by_variable_uuid_) + , problem_options_(other.problem_options_) + , variables_on_hold_(other.variables_on_hold_) { // Make a deep copy of the constraints - std::transform( - other.constraints_.begin(), - other.constraints_.end(), - std::inserter(constraints_, constraints_.end()), - [](const Constraints::value_type & uuid__constraint) -> Constraints::value_type - { - return {uuid__constraint.first, uuid__constraint.second->clone()}; - }); // NOLINT(whitespace/braces) + std::transform(other.constraints_.begin(), other.constraints_.end(), std::inserter(constraints_, constraints_.end()), + [](const Constraints::value_type& uuid__constraint) -> Constraints::value_type { + return { uuid__constraint.first, uuid__constraint.second->clone() }; + }); // NOLINT(whitespace/braces) // Make a deep copy of the variables - std::transform( - other.variables_.begin(), - other.variables_.end(), - std::inserter(variables_, variables_.end()), - [](const Variables::value_type & uuid__variable) -> Variables::value_type - { - return {uuid__variable.first, uuid__variable.second->clone()}; - }); // NOLINT(whitespace/braces) + std::transform(other.variables_.begin(), other.variables_.end(), std::inserter(variables_, variables_.end()), + [](const Variables::value_type& uuid__variable) -> Variables::value_type { + return { uuid__variable.first, uuid__variable.second->clone() }; + }); // NOLINT(whitespace/braces) } -HashGraph & HashGraph::operator=(const HashGraph & other) +HashGraph& HashGraph::operator=(const HashGraph& other) { // Make a copy (might throw an exception) HashGraph tmp(other); @@ -108,7 +99,7 @@ fuse_core::Graph::UniquePtr HashGraph::clone() const return HashGraph::make_unique(*this); } -bool HashGraph::constraintExists(const fuse_core::UUID & constraint_uuid) const noexcept +bool HashGraph::constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept { // map.find() does not itself throw exceptions, but may as a result of the key comparison // operator. Because the UUID comparisons are marked as noexcept by Boost, I feel safe marking @@ -120,103 +111,101 @@ bool HashGraph::constraintExists(const fuse_core::UUID & constraint_uuid) const bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) { // Do nothing if the constraint is empty, or the constraint already exists - if (!constraint || constraintExists(constraint->uuid())) { + if (!constraint || constraintExists(constraint->uuid())) + { return false; } // Check that all of the referenced variables exist. Throw a logic_error if they do not. - for (const auto & variable_uuid : constraint->variables()) { - if (!variableExists(variable_uuid)) { - throw std::logic_error( - "Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + - ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + - ")."); + for (const auto& variable_uuid : constraint->variables()) + { + if (!variableExists(variable_uuid)) + { + throw std::logic_error("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + + ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + ")."); } } // Add the constraint to the list of known constraints constraints_.emplace(constraint->uuid(), constraint); // Also add it to the variable-constraint cross reference - for (const auto & variable_uuid : constraint->variables()) { + for (const auto& variable_uuid : constraint->variables()) + { constraints_by_variable_uuid_[variable_uuid].push_back(constraint->uuid()); } return true; } -bool HashGraph::removeConstraint(const fuse_core::UUID & constraint_uuid) +bool HashGraph::removeConstraint(const fuse_core::UUID& constraint_uuid) { // Check if the constraint exists auto constraints_iter = constraints_.find(constraint_uuid); - if (constraints_iter == constraints_.end()) { + if (constraints_iter == constraints_.end()) + { return false; } // Remove the constraint from the cross-reference data structure - for (const auto & variable_uuid : constraints_iter->second->variables()) { - auto & constraints = constraints_by_variable_uuid_.at(variable_uuid); - constraints.erase( - std::remove( - constraints.begin(), - constraints.end(), constraint_uuid), constraints.end()); + for (const auto& variable_uuid : constraints_iter->second->variables()) + { + auto& constraints = constraints_by_variable_uuid_.at(variable_uuid); + constraints.erase(std::remove(constraints.begin(), constraints.end(), constraint_uuid), constraints.end()); } // And remove the constraint constraints_.erase(constraints_iter); // This does not throw return true; } -const fuse_core::Constraint & HashGraph::getConstraint(const fuse_core::UUID & constraint_uuid) -const +const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& constraint_uuid) const { auto constraints_iter = constraints_.find(constraint_uuid); - if (constraints_iter == constraints_.end()) { - throw std::out_of_range( - "The constraint UUID " + fuse_core::uuid::to_string( - constraint_uuid) + " does not exist."); + if (constraints_iter == constraints_.end()) + { + throw std::out_of_range("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); } return *constraints_iter->second; } fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexcept { - std::function - to_constraint_ref = - [](const Constraints::value_type & uuid__constraint) -> const fuse_core::Constraint & - { - return *uuid__constraint.second; - }; + std::function to_constraint_ref = + [](const Constraints::value_type& uuid__constraint) -> const fuse_core::Constraint& { + return *uuid__constraint.second; + }; return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(constraints_.cend(), to_constraint_ref)); + boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(constraints_.cend(), to_constraint_ref)); } -fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints( - const fuse_core::UUID & variable_uuid) const +fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(const fuse_core::UUID& variable_uuid) const { auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); - if (cross_reference_iter != constraints_by_variable_uuid_.end()) { - std::function - uuid_to_constraint_ref = - [this](const fuse_core::UUID & constraint_uuid) -> const fuse_core::Constraint & - { - return this->getConstraint(constraint_uuid); - }; + if (cross_reference_iter != constraints_by_variable_uuid_.end()) + { + std::function uuid_to_constraint_ref = + [this](const fuse_core::UUID& constraint_uuid) -> const fuse_core::Constraint& { + return this->getConstraint(constraint_uuid); + }; - const auto & constraints = cross_reference_iter->second; + const auto& constraints = cross_reference_iter->second; return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), - boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref)); - } else if (variableExists(variable_uuid)) { + boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), + boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref)); + } + else if (variableExists(variable_uuid)) + { // User requested a valid variable, but there are no attached constraints. Return an empty // range. return fuse_core::Graph::const_constraint_range(); - } else { + } + else + { // We only want to throw if the requested variable does not exist. - throw std::logic_error( - "Attempting to access constraints connected to variable (" + - fuse_core::uuid::to_string( - variable_uuid) + "), but that variable does not exist in this graph."); + throw std::logic_error("Attempting to access constraints connected to variable (" + + fuse_core::uuid::to_string(variable_uuid) + + "), but that variable does not exist in this graph."); } } -bool HashGraph::variableExists(const fuse_core::UUID & variable_uuid) const noexcept +bool HashGraph::variableExists(const fuse_core::UUID& variable_uuid) const noexcept { auto variables_iter = variables_.find(variable_uuid); return variables_iter != variables_.end(); @@ -225,92 +214,90 @@ bool HashGraph::variableExists(const fuse_core::UUID & variable_uuid) const noex bool HashGraph::addVariable(fuse_core::Variable::SharedPtr variable) { // Do nothing if the variable is empty, or the variable already exists - if (!variable || variableExists(variable->uuid())) { + if (!variable || variableExists(variable->uuid())) + { return false; } variables_.emplace(variable->uuid(), variable); - if (variable->holdConstant()) { + if (variable->holdConstant()) + { variables_on_hold_.insert(variable->uuid()); } return true; } -bool HashGraph::removeVariable(const fuse_core::UUID & variable_uuid) +bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) { // Check if the variable exists auto variables_iter = variables_.find(variable_uuid); - if (variables_iter == variables_.end()) { + if (variables_iter == variables_.end()) + { return false; } // Check that this variable is not used by any constraint. Throw a logic_error if the variable is // currently used. auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); - if (cross_reference_iter != constraints_by_variable_uuid_.end() && - !cross_reference_iter->second.empty()) + if (cross_reference_iter != constraints_by_variable_uuid_.end() && !cross_reference_iter->second.empty()) { - throw std::logic_error( - "Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + - ") that is used by existing constraints (" + - fuse_core::uuid::to_string(cross_reference_iter->second.front()) + - " plus " + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); + throw std::logic_error("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + + ") that is used by existing constraints (" + + fuse_core::uuid::to_string(cross_reference_iter->second.front()) + " plus " + + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); } // Remove the variable from all containers variables_.erase(variables_iter); // Does not throw - if (cross_reference_iter != constraints_by_variable_uuid_.end()) { + if (cross_reference_iter != constraints_by_variable_uuid_.end()) + { constraints_by_variable_uuid_.erase(cross_reference_iter); } variables_on_hold_.erase(variable_uuid); return true; } -const fuse_core::Variable & HashGraph::getVariable(const fuse_core::UUID & variable_uuid) const +const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variable_uuid) const { auto variables_iter = variables_.find(variable_uuid); - if (variables_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string( - variable_uuid) + " does not exist."); + if (variables_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); } return *variables_iter->second; } fuse_core::Graph::const_variable_range HashGraph::getVariables() const noexcept { - std::function - to_variable_ref = - [](const Variables::value_type & uuid__variable) -> const fuse_core::Variable & - { - return *uuid__variable.second; - }; + std::function to_variable_ref = + [](const Variables::value_type& uuid__variable) -> const fuse_core::Variable& { return *uuid__variable.second; }; - return fuse_core::Graph::const_variable_range( - boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(variables_.cend(), to_variable_ref)); + return fuse_core::Graph::const_variable_range(boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(variables_.cend(), to_variable_ref)); } -void HashGraph::holdVariable(const fuse_core::UUID & variable_uuid, bool hold_constant) +void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant) { // Adjust the variable setting in the Ceres Problem object - if (hold_constant) { + if (hold_constant) + { variables_on_hold_.insert(variable_uuid); - } else { + } + else + { variables_on_hold_.erase(variable_uuid); } } -bool HashGraph::isVariableOnHold(const fuse_core::UUID & variable_uuid) const +bool HashGraph::isVariableOnHold(const fuse_core::UUID& variable_uuid) const { return variables_on_hold_.find(variable_uuid) != variables_on_hold_.end(); } -void HashGraph::getCovariance( - const std::vector> & covariance_requests, - std::vector> & covariance_matrices, - const ceres::Covariance::Options & options, - const bool use_tangent_space) const +void HashGraph::getCovariance(const std::vector>& covariance_requests, + std::vector>& covariance_matrices, + const ceres::Covariance::Options& options, const bool use_tangent_space) const { // Avoid doing a bunch of work if the request is empty - if (covariance_requests.empty()) { + if (covariance_requests.empty()) + { return; } // Construct the ceres::Problem object from scratch @@ -319,103 +306,96 @@ void HashGraph::getCovariance( // The Ceres interface requires that the variable pairs not contain duplicates. Since the // covariance matrix is symmetric, requesting Cov(A,B) and Cov(B,A) counts as a duplicate. Create // an expression to test a pair of data pointers such that (A,B) == (A,B) OR (B,A) - auto symmetric_equal = [](const std::pair & x, - const std::pair & y) - { - return ((x.first == y.first) && (x.second == y.second)) || - ((x.first == y.second) && (x.second == y.first)); - }; + auto symmetric_equal = [](const std::pair& x, + const std::pair& y) { + return ((x.first == y.first) && (x.second == y.second)) || ((x.first == y.second) && (x.second == y.first)); + }; // Convert the covariance requests into the input structure needed by Ceres. Namely, we must // convert the variable UUIDs into memory addresses. We create two containers of covariance // blocks: one only contains the unique variable pairs that we give to Ceres, and a second that // contains all requested variable pairs used to keep the output structure in sync with the // request structure. - std::vector> unique_covariance_blocks; - std::vector> all_covariance_blocks; + std::vector> unique_covariance_blocks; + std::vector> all_covariance_blocks; all_covariance_blocks.resize(covariance_requests.size()); covariance_matrices.resize(covariance_requests.size()); - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & request = covariance_requests.at(i); + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& request = covariance_requests.at(i); auto variable1_iter = variables_.find(request.first); - if (variable1_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string(request.first) + - " does not exist."); + if (variable1_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.first) + " does not exist."); } auto variable2_iter = variables_.find(request.second); - if (variable2_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string(request.second) + - " does not exist."); + if (variable2_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.second) + " does not exist."); } // Both variables exist. Continue processing. // Create the output covariance matrix - if (use_tangent_space) { - covariance_matrices[i].resize( - variable1_iter->second->localSize() * variable2_iter->second->localSize()); - } else { - covariance_matrices[i].resize( - variable1_iter->second->size() * - variable2_iter->second->size()); + if (use_tangent_space) + { + covariance_matrices[i].resize(variable1_iter->second->localSize() * variable2_iter->second->localSize()); + } + else + { + covariance_matrices[i].resize(variable1_iter->second->size() * variable2_iter->second->size()); } // Add this covariance block to the container of all covariance blocks. This container is in // sync with the covariance_requests vector. - auto & block = all_covariance_blocks.at(i); + auto& block = all_covariance_blocks.at(i); block.first = variable1_iter->second->data(); block.second = variable2_iter->second->data(); // Also maintain a container of unique covariance blocks. Since the covariance matrix is // symmetric, requesting Cov(X,Y) and Cov(Y,X) counts as a duplicate, so we use our special // symmetric_equal function to test. - if (std::none_of( - unique_covariance_blocks.begin(), - unique_covariance_blocks.end(), - std::bind(symmetric_equal, block, std::placeholders::_1))) + if (std::none_of(unique_covariance_blocks.begin(), unique_covariance_blocks.end(), + std::bind(symmetric_equal, block, std::placeholders::_1))) { unique_covariance_blocks.push_back(block); } } // Call the Ceres function to compute the unique set of requested covariance blocks ceres::Covariance covariance(options); - if (!covariance.Compute(unique_covariance_blocks, &problem)) { + if (!covariance.Compute(unique_covariance_blocks, &problem)) + { throw std::runtime_error("Could not compute requested covariance blocks."); } // Populate the computed covariance blocks into the output variable. - if (use_tangent_space) { - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & block = all_covariance_blocks.at(i); - auto & output_matrix = covariance_matrices.at(i); - if (!covariance.GetCovarianceBlockInTangentSpace( - block.first, - block.second, - output_matrix.data())) + if (use_tangent_space) + { + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& block = all_covariance_blocks.at(i); + auto& output_matrix = covariance_matrices.at(i); + if (!covariance.GetCovarianceBlockInTangentSpace(block.first, block.second, output_matrix.data())) { - const auto & request = covariance_requests.at(i); - throw std::runtime_error( - "Could not get covariance block for variable UUIDs " + - fuse_core::uuid::to_string(request.first) + " and " + - fuse_core::uuid::to_string(request.second) + "."); + const auto& request = covariance_requests.at(i); + throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::uuid::to_string(request.first) + " and " + + fuse_core::uuid::to_string(request.second) + "."); } } - } else { - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & block = all_covariance_blocks.at(i); - auto & output_matrix = covariance_matrices.at(i); - if (!covariance.GetCovarianceBlock( - block.first, - block.second, - output_matrix.data())) + } + else + { + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& block = all_covariance_blocks.at(i); + auto& output_matrix = covariance_matrices.at(i); + if (!covariance.GetCovarianceBlock(block.first, block.second, output_matrix.data())) { - const auto & request = covariance_requests.at(i); - throw std::runtime_error( - "Could not get covariance block for variable UUIDs " + - fuse_core::uuid::to_string(request.first) + " and " + - fuse_core::uuid::to_string(request.second) + "."); + const auto& request = covariance_requests.at(i); + throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::uuid::to_string(request.first) + " and " + + fuse_core::uuid::to_string(request.second) + "."); } } } } -ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options & options) +ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options& options) { // Construct the ceres::Problem object from scratch ceres::Problem problem(problem_options_); @@ -427,10 +407,8 @@ ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options & option return summary; } -ceres::Solver::Summary HashGraph::optimizeFor( - const rclcpp::Duration & max_optimization_time, - const ceres::Solver::Options & options, - rclcpp::Clock clock) +ceres::Solver::Summary HashGraph::optimizeFor(const rclcpp::Duration& max_optimization_time, + const ceres::Solver::Options& options, rclcpp::Clock clock) { auto start = clock.now(); @@ -452,9 +430,8 @@ ceres::Solver::Summary HashGraph::optimizeFor( return summary; } -bool HashGraph::evaluate( - double * cost, std::vector * residuals, std::vector * gradient, - const ceres::Problem::EvaluateOptions & options) const +bool HashGraph::evaluate(double* cost, std::vector* residuals, std::vector* gradient, + const ceres::Problem::EvaluateOptions& options) const { ceres::Problem problem(problem_options_); createProblem(problem); @@ -462,15 +439,17 @@ bool HashGraph::evaluate( return problem.Evaluate(options, cost, residuals, gradient, nullptr); } -void HashGraph::print(std::ostream & stream) const +void HashGraph::print(std::ostream& stream) const { stream << "HashGraph\n" << " constraints:\n"; - for (const auto & constraint : constraints_) { + for (const auto& constraint : constraints_) + { stream << " - " << *constraint.second << "\n"; } stream << " variables:\n"; - for (const auto & variable : variables_) { + for (const auto& variable : variables_) + { const auto is_on_hold = variables_on_hold_.find(variable.first) != variables_on_hold_.end(); stream << " - " << *variable.second << "\n" @@ -478,49 +457,51 @@ void HashGraph::print(std::ostream & stream) const } } -void HashGraph::createProblem(ceres::Problem & problem) const +void HashGraph::createProblem(ceres::Problem& problem) const { // Add all the variables to the problem - for (auto & uuid__variable : variables_) { - fuse_core::Variable & variable = *(uuid__variable.second); - problem.AddParameterBlock( - variable.data(), - variable.size(), + for (auto& uuid__variable : variables_) + { + fuse_core::Variable& variable = *(uuid__variable.second); + problem.AddParameterBlock(variable.data(), variable.size(), #if !CERES_SUPPORTS_MANIFOLDS - variable.localParameterization()); + variable.localParameterization()); #else - variable.manifold()); + variable.manifold()); #endif // Handle optimization bounds - for (size_t index = 0; index < variable.size(); ++index) { + for (size_t index = 0; index < variable.size(); ++index) + { auto lower_bound = variable.lowerBound(index); - if (lower_bound > std::numeric_limits::lowest()) { + if (lower_bound > std::numeric_limits::lowest()) + { problem.SetParameterLowerBound(variable.data(), index, lower_bound); } auto upper_bound = variable.upperBound(index); - if (upper_bound < std::numeric_limits::max()) { + if (upper_bound < std::numeric_limits::max()) + { problem.SetParameterUpperBound(variable.data(), index, upper_bound); } } // Handle variables that are held constant - if (variables_on_hold_.find(variable.uuid()) != variables_on_hold_.end()) { + if (variables_on_hold_.find(variable.uuid()) != variables_on_hold_.end()) + { problem.SetParameterBlockConstant(variable.data()); } } // Add the constraints - std::vector parameter_blocks; - for (auto & uuid__constraint : constraints_) { - fuse_core::Constraint & constraint = *(uuid__constraint.second); + std::vector parameter_blocks; + for (auto& uuid__constraint : constraints_) + { + fuse_core::Constraint& constraint = *(uuid__constraint.second); // We need the memory address of each variable value referenced by this constraint parameter_blocks.clear(); parameter_blocks.reserve(constraint.variables().size()); - for (const auto & uuid : constraint.variables()) { + for (const auto& uuid : constraint.variables()) + { parameter_blocks.push_back(variables_.at(uuid)->data()); } - problem.AddResidualBlock( - constraint.costFunction(), - constraint.lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint.costFunction(), constraint.lossFunction(), parameter_blocks); } } diff --git a/fuse_graphs/test/covariance_constraint.hpp b/fuse_graphs/test/covariance_constraint.hpp index f525424ce..a63a9cf09 100644 --- a/fuse_graphs/test/covariance_constraint.hpp +++ b/fuse_graphs/test/covariance_constraint.hpp @@ -48,7 +48,6 @@ #include #include - /** * @brief Create a cost function that implements one of the Ceres unit tests * @@ -71,10 +70,7 @@ class CovarianceCostFunction : public ceres::CostFunction mutable_parameter_block_sizes()->push_back(1); } - bool Evaluate( - double const * const * /*parameters*/, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* /*parameters*/, double* residuals, double** jacobians) const override { residuals[0] = 1; residuals[1] = 1; @@ -85,47 +81,23 @@ class CovarianceCostFunction : public ceres::CostFunction residuals[6] = 2; residuals[7] = 2; - if (jacobians != NULL) { - if (jacobians[0] != NULL) { - static const double jacobian0[] = - { - 1.0, 0.0, - 0.0, 1.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - -5.0, -6.0, - 3.0, -2.0 - }; + if (jacobians != NULL) + { + if (jacobians[0] != NULL) + { + static const double jacobian0[] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, -5.0, -6.0, 3.0, -2.0 }; std::copy(jacobian0, jacobian0 + 16, jacobians[0]); } - if (jacobians[1] != NULL) { - static const double jacobian1[] = - { - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0, - 0.0, 0.0, 0.0, - 1.0, 2.0, 3.0, - 0.0, 0.0, 0.0 - }; + if (jacobians[1] != NULL) + { + static const double jacobian1[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 }; std::copy(jacobian1, jacobian1 + 24, jacobians[1]); } - if (jacobians[2] != NULL) { - static const double jacobian2[] = - { - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 5.0, - 0.0, - 2.0 - }; + if (jacobians[2] != NULL) + { + static const double jacobian2[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 2.0 }; std::copy(jacobian2, jacobian2 + 8, jacobians[2]); } } @@ -144,17 +116,19 @@ class CovarianceConstraint : public fuse_core::Constraint CovarianceConstraint() = default; - CovarianceConstraint( - const std::string & source, - const fuse_core::UUID & variable1_uuid, - const fuse_core::UUID & variable2_uuid, - const fuse_core::UUID & variable3_uuid) - : fuse_core::Constraint(source, {variable1_uuid, variable2_uuid, variable3_uuid}) + CovarianceConstraint(const std::string& source, const fuse_core::UUID& variable1_uuid, + const fuse_core::UUID& variable2_uuid, const fuse_core::UUID& variable3_uuid) + : fuse_core::Constraint(source, { variable1_uuid, variable2_uuid, variable3_uuid }) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override {return new CovarianceCostFunction();} + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override + { + return new CovarianceCostFunction(); + } private: // Allow Boost Serialization access to private methods @@ -167,10 +141,10 @@ class CovarianceConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_graphs/test/example_constraint.hpp b/fuse_graphs/test/example_constraint.hpp index 45d222d73..bbaa07937 100644 --- a/fuse_graphs/test/example_constraint.hpp +++ b/fuse_graphs/test/example_constraint.hpp @@ -47,20 +47,18 @@ #include #include - /** * @brief Dummy cost function used for testing */ class ExampleFunctor { public: - explicit ExampleFunctor(const double & b) - : b_(b) + explicit ExampleFunctor(const double& b) : b_(b) { } - template - bool operator()(const T * const variable, T * residual) const + template + bool operator()(const T* const variable, T* residual) const { residual[0] = variable[0] - T(b_); return true; @@ -80,14 +78,17 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - explicit ExampleConstraint(const std::string & source, const fuse_core::UUID & variable_uuid) - : fuse_core::Constraint(source, {variable_uuid}), // NOLINT + explicit ExampleConstraint(const std::string& source, const fuse_core::UUID& variable_uuid) + : fuse_core::Constraint(source, { variable_uuid }) + , // NOLINT data(0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override { return new ceres::AutoDiffCostFunction(new ExampleFunctor(data)); } @@ -105,11 +106,11 @@ class ExampleConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_graphs/test/example_loss.hpp b/fuse_graphs/test/example_loss.hpp index bdffbb09c..fdc8ea883 100644 --- a/fuse_graphs/test/example_loss.hpp +++ b/fuse_graphs/test/example_loss.hpp @@ -45,7 +45,6 @@ #include #include - /** * @brief Dummy loss implementation for testing */ @@ -54,27 +53,27 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) - : a(a) + explicit ExampleLoss(const double a = 1.0) : a(a) { } void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override {} + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::LossFunction * lossFunction() const override + ceres::LossFunction* lossFunction() const override { return new ceres::HuberLoss(a); } - double a{1.0}; //!< Public member variable just for testing + double a{ 1.0 }; //!< Public member variable just for testing private: // Allow Boost Serialization access to private methods @@ -87,11 +86,11 @@ class ExampleLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a; + archive& boost::serialization::base_object(*this); + archive& a; } }; diff --git a/fuse_graphs/test/example_variable.hpp b/fuse_graphs/test/example_variable.hpp index eccfd56fd..ba0bdca6e 100644 --- a/fuse_graphs/test/example_variable.hpp +++ b/fuse_graphs/test/example_variable.hpp @@ -45,7 +45,6 @@ #include #include - /** * @brief Dummy variable implementation for testing */ @@ -54,16 +53,25 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - explicit ExampleVariable(size_t N = 1) - : fuse_core::Variable(fuse_core::uuid::generate()), - data_(N, 0.0) + explicit ExampleVariable(size_t N = 1) : fuse_core::Variable(fuse_core::uuid::generate()), data_(N, 0.0) { } - size_t size() const override {return data_.size();} - const double * data() const override {return data_.data();} - double * data() override {return data_.data();} - void print(std::ostream & /*stream = std::cout*/) const override {} + size_t size() const override + { + return data_.size(); + } + const double* data() const override + { + return data_.data(); + } + double* data() override + { + return data_.data(); + } + void print(std::ostream& /*stream = std::cout*/) const override + { + } private: std::vector data_; @@ -78,11 +86,11 @@ class ExampleVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_graphs/test/test_hash_graph.cpp b/fuse_graphs/test/test_hash_graph.cpp index 97c6cfc83..484f01dfb 100644 --- a/fuse_graphs/test/test_hash_graph.cpp +++ b/fuse_graphs/test/test_hash_graph.cpp @@ -65,36 +65,51 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual variable * @return True if all the properties match, false otherwise */ - bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Variable & actual) + bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) { failure_description = ""; bool variables_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { variables_equal = false; failure_description += "The variables have different types.\n" - " expected type is '" + expected.type() + "'\n" - " actual type is '" + actual.type() + "'\n"; + " expected type is '" + + expected.type() + + "'\n" + " actual type is '" + + actual.type() + "'\n"; } - if (expected.size() != actual.size()) { + if (expected.size() != actual.size()) + { variables_equal = false; failure_description += "The variables have different sizes.\n" - " expected size is '" + std::to_string(expected.size()) + "'\n" - " actual size is '" + std::to_string(actual.size()) + "'\n"; + " expected size is '" + + std::to_string(expected.size()) + + "'\n" + " actual size is '" + + std::to_string(actual.size()) + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { variables_equal = false; failure_description += "The variables have different UUIDs.\n" - " expected UUID is '" + fuse_core::uuid::to_string(expected.uuid()) + "'\n" - " actual UUID is '" + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + " expected UUID is '" + + fuse_core::uuid::to_string(expected.uuid()) + + "'\n" + " actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - for (size_t i = 0; i < expected.size(); ++i) { - if (expected.data()[i] != actual.data()[i]) { + for (size_t i = 0; i < expected.size(); ++i) + { + if (expected.data()[i] != actual.data()[i]) + { variables_equal = false; failure_description += "The variables have different values.\n" - " expected data(" + std::to_string(i) + ") is '" + std::to_string(expected.data()[i]) + - "'\n" - " actual data(" + std::to_string(i) + ") is '" + std::to_string(actual.data()[i]) + - "'\n"; + " expected data(" + + std::to_string(i) + ") is '" + std::to_string(expected.data()[i]) + + "'\n" + " actual data(" + + std::to_string(i) + ") is '" + std::to_string(actual.data()[i]) + "'\n"; } } return variables_equal; @@ -107,46 +122,59 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual constraint * @return True if all the properties match, false otherwise */ - bool compareConstraints( - const fuse_core::Constraint & expected, - const fuse_core::Constraint & actual) + bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) { failure_description = ""; bool constraints_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { constraints_equal = false; failure_description += "The constraints have different types.\n" - " expected type is '" + expected.type() + "'\n" - " actual type is '" + actual.type() + "'\n"; + " expected type is '" + + expected.type() + + "'\n" + " actual type is '" + + actual.type() + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { constraints_equal = false; failure_description += "The constraints have different UUIDs.\n" - " expected UUID is '" + fuse_core::uuid::to_string(expected.uuid()) + "'\n" - " actual UUID is '" + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + " expected UUID is '" + + fuse_core::uuid::to_string(expected.uuid()) + + "'\n" + " actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - if (expected.variables().size() != actual.variables().size()) { + if (expected.variables().size() != actual.variables().size()) + { constraints_equal = false; failure_description += "The constraints involve a different number of variables.\n" - " expected variable count is '" + std::to_string(expected.variables().size()) + "'\n" - " actual variable count is '" + std::to_string(actual.variables().size()) + "'\n"; + " expected variable count is '" + + std::to_string(expected.variables().size()) + + "'\n" + " actual variable count is '" + + std::to_string(actual.variables().size()) + "'\n"; } - for (size_t i = 0; i < expected.variables().size(); ++i) { - if (expected.variables().at(i) != actual.variables().at(i)) { + for (size_t i = 0; i < expected.variables().size(); ++i) + { + if (expected.variables().at(i) != actual.variables().at(i)) + { constraints_equal = false; std::string i_str = std::to_string(i); failure_description += "The constraints involve different variable UUIDs.\n" - " expected variables(" + i_str + ") is '" + fuse_core::uuid::to_string( - expected.variables()[i]) + "'\n" - " actual variables(" + i_str + ") is '" + fuse_core::uuid::to_string( - actual.variables()[i]) + "'\n"; + " expected variables(" + + i_str + ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + + "'\n" + " actual variables(" + + i_str + ") is '" + fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; } } return constraints_equal; } - std::string failure_description {""}; //!< The last failure description. Empty if no failure - //!< happened + std::string failure_description{ "" }; //!< The last failure description. Empty if no failure + //!< happened }; TEST_F(HashGraphTestFixture, AddVariable) @@ -272,10 +300,10 @@ TEST_F(HashGraphTestFixture, GetVariable) graph.addVariable(variable2); // Verify all of the variables are available - const fuse_core::Variable & actual1 = graph.getVariable(variable1->uuid()); + const fuse_core::Variable& actual1 = graph.getVariable(variable1->uuid()); EXPECT_TRUE(compareVariables(*variable1, actual1)) << failure_description; - const fuse_core::Variable & actual2 = graph.getVariable(variable2->uuid()); + const fuse_core::Variable& actual2 = graph.getVariable(variable2->uuid()); EXPECT_TRUE(compareVariables(*variable2, actual2)) << failure_description; } @@ -304,16 +332,20 @@ TEST_F(HashGraphTestFixture, GetVariables) ASSERT_EQ(3, std::distance(variables.begin(), variables.end())); // Verify we received the correct variables - for (const auto & actual : variables) { - if (actual.uuid() == variable1->uuid()) { + for (const auto& actual : variables) + { + if (actual.uuid() == variable1->uuid()) + { EXPECT_TRUE(compareVariables(*variable1, actual)) << failure_description; continue; } - if (actual.uuid() == variable2->uuid()) { + if (actual.uuid() == variable2->uuid()) + { EXPECT_TRUE(compareVariables(*variable2, actual)) << failure_description; continue; } - if (actual.uuid() == variable3->uuid()) { + if (actual.uuid() == variable3->uuid()) + { EXPECT_TRUE(compareVariables(*variable3, actual)) << failure_description; continue; } @@ -352,8 +384,10 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint1->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto & actual : actual_variables) { - if (actual.uuid() == variable1->uuid()) { + for (const auto& actual : actual_variables) + { + if (actual.uuid() == variable1->uuid()) + { EXPECT_TRUE(compareVariables(*variable1, actual)) << failure_description; continue; } @@ -365,8 +399,10 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint2->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto & actual : actual_variables) { - if (actual.uuid() == variable2->uuid()) { + for (const auto& actual : actual_variables) + { + if (actual.uuid() == variable2->uuid()) + { EXPECT_TRUE(compareVariables(*variable2, actual)) << failure_description; continue; } @@ -511,10 +547,10 @@ TEST_F(HashGraphTestFixture, GetConstraint) graph.addConstraint(constraint2); // Verify all of the constraints are available - const fuse_core::Constraint & actual1 = graph.getConstraint(constraint1->uuid()); + const fuse_core::Constraint& actual1 = graph.getConstraint(constraint1->uuid()); EXPECT_TRUE(compareConstraints(*constraint1, actual1)) << failure_description; - const fuse_core::Constraint & actual2 = graph.getConstraint(constraint2->uuid()); + const fuse_core::Constraint& actual2 = graph.getConstraint(constraint2->uuid()); EXPECT_TRUE(compareConstraints(*constraint2, actual2)) << failure_description; } @@ -547,16 +583,20 @@ TEST_F(HashGraphTestFixture, GetConstraints) ASSERT_EQ(3, std::distance(constraints.begin(), constraints.end())); // Verify we received the correct constraints - for (const auto & actual : constraints) { - if (actual.uuid() == constraint1->uuid()) { + for (const auto& actual : constraints) + { + if (actual.uuid() == constraint1->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint1, actual)) << failure_description; continue; } - if (actual.uuid() == constraint2->uuid()) { + if (actual.uuid() == constraint2->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint2, actual)) << failure_description; continue; } - if (actual.uuid() == constraint3->uuid()) { + if (actual.uuid() == constraint3->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint3, actual)) << failure_description; continue; } @@ -598,32 +638,35 @@ TEST_F(HashGraphTestFixture, GetConnectedConstraints) { auto actual_constraints = graph.getConnectedConstraints(variable1->uuid()); ASSERT_EQ(2, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto & actual : actual_constraints) { - if (actual.uuid() == constraint1->uuid()) { + for (const auto& actual : actual_constraints) + { + if (actual.uuid() == constraint1->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint1, actual)) << failure_description; continue; } - if (actual.uuid() == constraint3->uuid()) { + if (actual.uuid() == constraint3->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint3, actual)) << failure_description; continue; } // The constraint was not one of the expected constraints. Fail the test. - FAIL() << "The actual constraint '" << actual.uuid() << - "' was not in the expected collection."; + FAIL() << "The actual constraint '" << actual.uuid() << "' was not in the expected collection."; } } { auto actual_constraints = graph.getConnectedConstraints(variable2->uuid()); ASSERT_EQ(1, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto & actual : actual_constraints) { - if (actual.uuid() == constraint2->uuid()) { + for (const auto& actual : actual_constraints) + { + if (actual.uuid() == constraint2->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint2, actual)) << failure_description; continue; } // The constraint was not one of the expected constraints. Fail the test. - FAIL() << "The actual constraint '" << actual.uuid() << - "' was not in the expected collection."; + FAIL() << "The actual constraint '" << actual.uuid() << "' was not in the expected collection."; } } @@ -773,11 +816,11 @@ TEST_F(HashGraphTestFixture, GetCovariance) covariance_requests.emplace_back(z->uuid(), y->uuid()); std::vector> covariance_matrices; graph.getCovariance(covariance_requests, covariance_matrices); - const std::vector & actual0 = covariance_matrices.at(0); - const std::vector & actual1 = covariance_matrices.at(1); - const std::vector & actual2 = covariance_matrices.at(2); - const std::vector & actual3 = covariance_matrices.at(3); - const std::vector & actual4 = covariance_matrices.at(4); + const std::vector& actual0 = covariance_matrices.at(0); + const std::vector& actual1 = covariance_matrices.at(1); + const std::vector& actual2 = covariance_matrices.at(2); + const std::vector& actual3 = covariance_matrices.at(3); + const std::vector& actual4 = covariance_matrices.at(4); // Compare with the expected blocks // full covariance = { @@ -792,37 +835,39 @@ TEST_F(HashGraphTestFixture, GetCovariance) // [ XX (2x2), XY(2x3), XZ (2x1)] // [ YX (3x2), YY(3x3), YZ (3x1)] // [ ZX (1x2), ZY(1x3), ZZ (1x1)] - std::vector expected0 = {7.0747e-02, -8.4923e-03, -8.4923e-03, 8.1352e-02}; // XX - std::vector expected1 = - {1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY - std::vector expected2 = - {1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02}; // YX - std::vector expected3 = - {1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY - std::vector expected4 = {-6.5325e-05, -1.3065e-04, -1.9598e-04}; // ZY + std::vector expected0 = { 7.0747e-02, -8.4923e-03, -8.4923e-03, 8.1352e-02 }; // XX + std::vector expected1 = { 1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02 }; // XY + std::vector expected2 = { 1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02 }; // YX + std::vector expected3 = { 1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02 }; // XY + std::vector expected4 = { -6.5325e-05, -1.3065e-04, -1.9598e-04 }; // ZY ASSERT_EQ(expected0.size(), actual0.size()); - for (size_t i = 0; i < expected0.size(); ++i) { + for (size_t i = 0; i < expected0.size(); ++i) + { EXPECT_NEAR(expected0[i], actual0[i], 1.0e-5); } ASSERT_EQ(expected1.size(), actual1.size()); - for (size_t i = 0; i < expected1.size(); ++i) { + for (size_t i = 0; i < expected1.size(); ++i) + { EXPECT_NEAR(expected1[i], actual1[i], 1.0e-5); } ASSERT_EQ(expected2.size(), actual2.size()); - for (size_t i = 0; i < expected2.size(); ++i) { + for (size_t i = 0; i < expected2.size(); ++i) + { EXPECT_NEAR(expected2[i], actual2[i], 1.0e-5); } ASSERT_EQ(expected3.size(), actual3.size()); - for (size_t i = 0; i < expected3.size(); ++i) { + for (size_t i = 0; i < expected3.size(); ++i) + { EXPECT_NEAR(expected3[i], actual3[i], 1.0e-5); } ASSERT_EQ(expected4.size(), actual4.size()); - for (size_t i = 0; i < expected4.size(); ++i) { + for (size_t i = 0; i < expected4.size(); ++i) + { EXPECT_NEAR(expected4[i], actual4[i], 1.0e-5); } } @@ -855,10 +900,12 @@ TEST_F(HashGraphTestFixture, Copy) { fuse_graphs::HashGraph other(graph); // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other.variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -873,10 +920,12 @@ TEST_F(HashGraphTestFixture, Copy) fuse_graphs::HashGraph other; other = graph; // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other.variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -890,10 +939,12 @@ TEST_F(HashGraphTestFixture, Copy) { auto other = graph.clone(); // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other->constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other->variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -942,10 +993,12 @@ TEST_F(HashGraphTestFixture, Serialization) } // Verify the copy - for (const auto & constraint : expected.getConstraints()) { + for (const auto& constraint : expected.getConstraints()) + { EXPECT_TRUE(actual.constraintExists(constraint.uuid())); } - for (const auto & variable : expected.getVariables()) { + for (const auto& variable : expected.getVariables()) + { EXPECT_TRUE(actual.variableExists(variable.uuid())); } } @@ -982,9 +1035,7 @@ TEST_F(HashGraphTestFixture, GetConstraintCosts) constraint_uuids.push_back(constraint1->uuid()); auto costs = std::vector(); - graph.getConstraintCosts( - constraint_uuids.begin(), constraint_uuids.end(), - std::back_inserter(costs)); + graph.getConstraintCosts(constraint_uuids.begin(), constraint_uuids.end(), std::back_inserter(costs)); ASSERT_EQ(costs.size(), 2u); EXPECT_NEAR(costs[0].cost, 2.5, 1.0e-5); diff --git a/fuse_loss/include/fuse_loss/arctan_loss.hpp b/fuse_loss/include/fuse_loss/arctan_loss.hpp index 692d0e84a..f83cdff0e 100644 --- a/fuse_loss/include/fuse_loss/arctan_loss.hpp +++ b/fuse_loss/include/fuse_loss/arctan_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class ArctanLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class ArctanLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class ArctanLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< ArctanLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< ArctanLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class ArctanLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/cauchy_loss.hpp b/fuse_loss/include/fuse_loss/cauchy_loss.hpp index d724c57c4..a4b8bb77e 100644 --- a/fuse_loss/include/fuse_loss/cauchy_loss.hpp +++ b/fuse_loss/include/fuse_loss/cauchy_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class CauchyLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class CauchyLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class CauchyLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< CauchyLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< CauchyLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class CauchyLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/composed_loss.hpp b/fuse_loss/include/fuse_loss/composed_loss.hpp index 049fc674a..62ac6dd80 100644 --- a/fuse_loss/include/fuse_loss/composed_loss.hpp +++ b/fuse_loss/include/fuse_loss/composed_loss.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_loss { @@ -72,9 +71,8 @@ class ComposedLoss : public fuse_core::Loss * 'f(g(s))'. If it is nullptr the fuse_loss::TrivialLoss is used. Defaults to * nullptr. */ - explicit ComposedLoss( - const std::shared_ptr & f_loss = nullptr, - const std::shared_ptr & g_loss = nullptr); + explicit ComposedLoss(const std::shared_ptr& f_loss = nullptr, + const std::shared_ptr& g_loss = nullptr); /** * @brief Destructor @@ -92,19 +90,17 @@ class ComposedLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function. @@ -117,7 +113,7 @@ class ComposedLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'f_loss' accessor. @@ -144,7 +140,7 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'f_loss'. */ - void fLoss(const std::shared_ptr & f_loss) + void fLoss(const std::shared_ptr& f_loss) { f_loss_ = f_loss; } @@ -154,16 +150,16 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'g_loss'. */ - void gLoss(const std::shared_ptr & g_loss) + void gLoss(const std::shared_ptr& g_loss) { g_loss_ = g_loss; } private: - std::shared_ptr f_loss_{nullptr}; //!< The 'f' loss function, which is + std::shared_ptr f_loss_{ nullptr }; //!< The 'f' loss function, which is //!< evaluated last to yield the composition //!< 'f(g(s))' - std::shared_ptr g_loss_{nullptr}; //!< The 'g' loss function, which is + std::shared_ptr g_loss_{ nullptr }; //!< The 'g' loss function, which is //!< evaluated first to yield the //!< composition 'f(g(s))' @@ -177,12 +173,12 @@ class ComposedLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & f_loss_; - archive & g_loss_; + archive& boost::serialization::base_object(*this); + archive& f_loss_; + archive& g_loss_; } }; diff --git a/fuse_loss/include/fuse_loss/dcs_loss.hpp b/fuse_loss/include/fuse_loss/dcs_loss.hpp index 1c5fe39c0..870933340 100644 --- a/fuse_loss/include/fuse_loss/dcs_loss.hpp +++ b/fuse_loss/include/fuse_loss/dcs_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class DCSLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class DCSLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class DCSLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< DCSLoss parameter 'a' + double a_{ 1.0 }; //!< DCSLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class DCSLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/fair_loss.hpp b/fuse_loss/include/fuse_loss/fair_loss.hpp index 9c81ef02f..d4a022c13 100644 --- a/fuse_loss/include/fuse_loss/fair_loss.hpp +++ b/fuse_loss/include/fuse_loss/fair_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class FairLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class FairLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class FairLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< FairLoss parameter 'a' + double a_{ 1.0 }; //!< FairLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class FairLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp index fc7a24408..265aaf12c 100644 --- a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp +++ b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class GemanMcClureLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class GemanMcClureLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class GemanMcClureLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< GemanMcClureLoss parameter 'a' + double a_{ 1.0 }; //!< GemanMcClureLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class GemanMcClureLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/huber_loss.hpp b/fuse_loss/include/fuse_loss/huber_loss.hpp index 920a376f5..e38b479da 100644 --- a/fuse_loss/include/fuse_loss/huber_loss.hpp +++ b/fuse_loss/include/fuse_loss/huber_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class HuberLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class HuberLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class HuberLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< HuberLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< HuberLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class HuberLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/loss_function.hpp b/fuse_loss/include/fuse_loss/loss_function.hpp index 553739c30..68ba629eb 100644 --- a/fuse_loss/include/fuse_loss/loss_function.hpp +++ b/fuse_loss/include/fuse_loss/loss_function.hpp @@ -145,12 +145,11 @@ namespace ceres class DCSLoss : public ceres::LossFunction { public: - explicit DCSLoss(const double a) - : a_(a) + explicit DCSLoss(const double a) : a_(a) { } - void Evaluate(double, double * rho) const override; + void Evaluate(double, double* rho) const override; private: const double a_; @@ -170,12 +169,11 @@ class DCSLoss : public ceres::LossFunction class FairLoss : public ceres::LossFunction { public: - explicit FairLoss(const double a) - : a_(a), b_(a * a) + explicit FairLoss(const double a) : a_(a), b_(a * a) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double a_; @@ -222,12 +220,11 @@ class FairLoss : public ceres::LossFunction class GemanMcClureLoss : public ceres::LossFunction { public: - explicit GemanMcClureLoss(const double a) - : b_(a * a) + explicit GemanMcClureLoss(const double a) : b_(a * a) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double b_; @@ -246,12 +243,11 @@ class GemanMcClureLoss : public ceres::LossFunction class WelschLoss : public ceres::LossFunction { public: - explicit WelschLoss(const double a) - : b_(a * a), c_(-1.0 / b_) + explicit WelschLoss(const double a) : b_(a * a), c_(-1.0 / b_) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double b_; diff --git a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp index 8d48986e9..267f28c52 100644 --- a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp +++ b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp @@ -62,7 +62,8 @@ class HSVColormap public: explicit HSVColormap(const size_t size = 256) { - if (size == 0) { + if (size == 0) + { return; } @@ -70,7 +71,8 @@ class HSVColormap double hue = 0.0; const double hue_increment = 1.0 / size; - for (size_t i = 0; i < size; ++i, hue += hue_increment) { + for (size_t i = 0; i < size; ++i, hue += hue_increment) + { QColor color; color.setHsvF(hue, 1.0, 1.0); @@ -78,7 +80,7 @@ class HSVColormap } } - const QColor & operator[](const size_t i) const + const QColor& operator[](const size_t i) const { return colormap_[i]; } @@ -95,12 +97,11 @@ class HSVColormap class LossEvaluator { public: - explicit LossEvaluator(const std::vector & residuals) - : residuals_(residuals) + explicit LossEvaluator(const std::vector& residuals) : residuals_(residuals) { } - std::vector rho(const ceres::LossFunction * loss_function) const + std::vector rho(const ceres::LossFunction* loss_function) const { std::vector rhos; rhos.reserve(residuals_.size()); @@ -113,18 +114,17 @@ class LossEvaluator // // See: https://github.com/ceres-solver/ceres- // solver/blob/master/internal/ceres/residual_block.cc#L165 - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(rhos), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return 0.5 * rho[0]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(rhos), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return 0.5 * rho[0]; + }); // NOLINT(whitespace/braces) return rhos; } - std::vector influence(const ceres::LossFunction * loss_function) const + std::vector influence(const ceres::LossFunction* loss_function) const { std::vector influence; influence.reserve(residuals_.size()); @@ -154,18 +154,17 @@ class LossEvaluator // // where \rho(r) = 0.5 * rho(r^2) is the inverse of rho(s) = 2 * \rho(sqrt(s)), // because s = r^2 and r = sqrt(s). - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(influence), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return r * rho[1]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(influence), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return r * rho[1]; + }); // NOLINT(whitespace/braces) return influence; } - std::vector weight(const ceres::LossFunction * loss_function) const + std::vector weight(const ceres::LossFunction* loss_function) const { std::vector weight; weight.reserve(residuals_.size()); @@ -181,18 +180,17 @@ class LossEvaluator // ds ds // // That is, rho[1]. - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(weight), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return rho[1]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(weight), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return rho[1]; + }); // NOLINT(whitespace/braces) return weight; } - std::vector secondDerivative(const ceres::LossFunction * loss_function) const + std::vector secondDerivative(const ceres::LossFunction* loss_function) const { std::vector second_derivative; second_derivative.reserve(residuals_.size()); @@ -204,18 +202,17 @@ class LossEvaluator // ds^2 // // That is, rho[2]. - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(second_derivative), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return rho[2]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(second_derivative), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return rho[2]; + }); // NOLINT(whitespace/braces) return second_derivative; } - const std::vector & getResiduals() const + const std::vector& getResiduals() const { return residuals_; } @@ -227,8 +224,8 @@ class LossEvaluator class QwtLossPlot { public: - QwtLossPlot(const std::vector & residuals, const HSVColormap & colormap) - : residuals_(QVector(residuals.begin(), residuals.end())) + QwtLossPlot(const std::vector& residuals, const HSVColormap& colormap) + : residuals_(QVector(residuals.begin(), residuals.end())) , loss_evaluator_(residuals) , colormap_(colormap) , magnifier_(plot_.canvas()) @@ -252,14 +249,14 @@ class QwtLossPlot plot_.insertLegend(&legend_); } - static std::string getName(const std::string & type) + static std::string getName(const std::string& type) { return type.substr(11, type.size() - 15); } - QwtPlotCurve * createCurve(const std::string & name, const std::vector & values) + QwtPlotCurve* createCurve(const std::string& name, const std::vector& values) { - QwtPlotCurve * curve = new QwtPlotCurve(name.c_str()); + QwtPlotCurve* curve = new QwtPlotCurve(name.c_str()); curve->setSamples(residuals_, QVector(values.begin(), values.end())); @@ -269,45 +266,33 @@ class QwtLossPlot return curve; } - void plotRho(const std::shared_ptr & loss) + void plotRho(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.rho(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.rho(loss->lossFunction()))); } - void plotInfluence(const std::shared_ptr & loss) + void plotInfluence(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.influence(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.influence(loss->lossFunction()))); } - void plotWeight(const std::shared_ptr & loss) + void plotWeight(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.weight(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.weight(loss->lossFunction()))); } - void plotSecondDerivative(const std::shared_ptr & loss) + void plotSecondDerivative(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.secondDerivative(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.secondDerivative(loss->lossFunction()))); } - void save(const std::string & filename) + void save(const std::string& filename) { QwtPlotRenderer renderer; renderer.renderDocument(&plot_, filename.c_str(), QSizeF(300, 200)); } - QwtPlot & plot() + QwtPlot& plot() { return plot_; } @@ -319,7 +304,7 @@ class QwtLossPlot HSVColormap colormap_; // We don't use an std::shared_ptr because QwtPlot takes ownership of the curves // attached to it and deletes them on destruction - std::vector curves_; + std::vector curves_; QwtPlot plot_; QwtPlotGrid grid_; diff --git a/fuse_loss/include/fuse_loss/scaled_loss.hpp b/fuse_loss/include/fuse_loss/scaled_loss.hpp index 6253f5fe4..a8ed6ccab 100644 --- a/fuse_loss/include/fuse_loss/scaled_loss.hpp +++ b/fuse_loss/include/fuse_loss/scaled_loss.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_loss { @@ -68,9 +67,7 @@ class ScaledLoss : public fuse_core::Loss * @param[in] a ScaledLoss parameter 'a'. See Ceres documentation for more details. * @param[in] loss The loss function to scale. Its output is scaled/multiplied by 'a'. */ - explicit ScaledLoss( - const double a = 1.0, - const std::shared_ptr & loss = nullptr); + explicit ScaledLoss(const double a = 1.0, const std::shared_ptr& loss = nullptr); /** * @brief Destructor @@ -88,19 +85,17 @@ class ScaledLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function. @@ -113,7 +108,7 @@ class ScaledLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -150,14 +145,14 @@ class ScaledLoss : public fuse_core::Loss * * @param[in] loss Parameter 'loss'. */ - void loss(const std::shared_ptr & loss) + void loss(const std::shared_ptr& loss) { loss_ = loss; } private: - double a_{1.0}; //!< ScaledLoss parameter 'a'. See Ceres documentation for more details - std::shared_ptr loss_{nullptr}; //!< The loss function to scale + double a_{ 1.0 }; //!< ScaledLoss parameter 'a'. See Ceres documentation for more details + std::shared_ptr loss_{ nullptr }; //!< The loss function to scale // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -169,12 +164,12 @@ class ScaledLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; - archive & loss_; + archive& boost::serialization::base_object(*this); + archive& a_; + archive& loss_; } }; diff --git a/fuse_loss/include/fuse_loss/softlone_loss.hpp b/fuse_loss/include/fuse_loss/softlone_loss.hpp index 840514f9b..740c7034b 100644 --- a/fuse_loss/include/fuse_loss/softlone_loss.hpp +++ b/fuse_loss/include/fuse_loss/softlone_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class SoftLOneLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class SoftLOneLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class SoftLOneLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< SoftLOneLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< SoftLOneLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class SoftLOneLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/tolerant_loss.hpp b/fuse_loss/include/fuse_loss/tolerant_loss.hpp index 75467c893..26cd8c9cb 100644 --- a/fuse_loss/include/fuse_loss/tolerant_loss.hpp +++ b/fuse_loss/include/fuse_loss/tolerant_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -85,19 +84,17 @@ class TolerantLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -110,7 +107,7 @@ class TolerantLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -153,8 +150,8 @@ class TolerantLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< TolerantLoss parameter 'a'. See Ceres documentation for more details - double b_{0.1}; //!< TolerantLoss parameter 'b'. See Ceres documentation for more details + double a_{ 1.0 }; //!< TolerantLoss parameter 'a'. See Ceres documentation for more details + double b_{ 0.1 }; //!< TolerantLoss parameter 'b'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -166,12 +163,12 @@ class TolerantLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; - archive & b_; + archive& boost::serialization::base_object(*this); + archive& a_; + archive& b_; } }; diff --git a/fuse_loss/include/fuse_loss/trivial_loss.hpp b/fuse_loss/include/fuse_loss/trivial_loss.hpp index 9cce011d3..130199cce 100644 --- a/fuse_loss/include/fuse_loss/trivial_loss.hpp +++ b/fuse_loss/include/fuse_loss/trivial_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -82,12 +81,9 @@ class TrivialLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override { } @@ -96,7 +92,7 @@ class TrivialLoss : public fuse_core::Loss * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +105,7 @@ class TrivialLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; private: // Allow Boost Serialization access to private methods @@ -122,10 +118,10 @@ class TrivialLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_loss/include/fuse_loss/tukey_loss.hpp b/fuse_loss/include/fuse_loss/tukey_loss.hpp index 7ef4b9c4f..8d43d516d 100644 --- a/fuse_loss/include/fuse_loss/tukey_loss.hpp +++ b/fuse_loss/include/fuse_loss/tukey_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class TukeyLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class TukeyLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class TukeyLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< TukeyLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< TukeyLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class TukeyLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/welsch_loss.hpp b/fuse_loss/include/fuse_loss/welsch_loss.hpp index 80f384c2d..092b8b681 100644 --- a/fuse_loss/include/fuse_loss/welsch_loss.hpp +++ b/fuse_loss/include/fuse_loss/welsch_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class WelschLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class WelschLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class WelschLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< WelschLoss parameter 'a' + double a_{ 1.0 }; //!< WelschLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class WelschLoss : public fuse_core::Loss * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp index ad1df8744..8736ea0c7 100644 --- a/fuse_loss/src/arctan_loss.cpp +++ b/fuse_loss/src/arctan_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -ArctanLoss::ArctanLoss(const double a) -: a_(a) +ArctanLoss::ArctanLoss(const double a) : a_(a) { } void ArctanLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void ArctanLoss::print(std::ostream & stream) const +void ArctanLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * ArctanLoss::lossFunction() const +ceres::LossFunction* ArctanLoss::lossFunction() const { return new ceres::ArctanLoss(a_); } diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp index a93066d06..372dd978b 100644 --- a/fuse_loss/src/cauchy_loss.cpp +++ b/fuse_loss/src/cauchy_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -CauchyLoss::CauchyLoss(const double a) -: a_(a) +CauchyLoss::CauchyLoss(const double a) : a_(a) { } void CauchyLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void CauchyLoss::print(std::ostream & stream) const +void CauchyLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * CauchyLoss::lossFunction() const +ceres::LossFunction* CauchyLoss::lossFunction() const { return new ceres::CauchyLoss(a_); } diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp index dc684adb3..bafb40940 100644 --- a/fuse_loss/src/composed_loss.cpp +++ b/fuse_loss/src/composed_loss.cpp @@ -44,43 +44,41 @@ namespace fuse_loss { -ComposedLoss::ComposedLoss( - const std::shared_ptr & f_loss, - const std::shared_ptr & g_loss) -: f_loss_(f_loss), g_loss_(g_loss) +ComposedLoss::ComposedLoss(const std::shared_ptr& f_loss, + const std::shared_ptr& g_loss) + : f_loss_(f_loss), g_loss_(g_loss) { } void ComposedLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { f_loss_ = fuse_core::loadLossConfig(interfaces, name + ".f_loss"); g_loss_ = fuse_core::loadLossConfig(interfaces, name + ".g_loss"); } -void ComposedLoss::print(std::ostream & stream) const +void ComposedLoss::print(std::ostream& stream) const { stream << type() << "\n"; - if (f_loss_) { + if (f_loss_) + { stream << " f_loss: " << f_loss_ << "\n"; } - if (g_loss_) { + if (g_loss_) + { stream << " g_loss: " << g_loss_ << "\n"; } } -ceres::LossFunction * ComposedLoss::lossFunction() const +ceres::LossFunction* ComposedLoss::lossFunction() const { - return new ceres::ComposedLoss( - f_loss_ ? f_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership, - g_loss_ ? g_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership); + return new ceres::ComposedLoss(f_loss_ ? f_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership, + g_loss_ ? g_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership); } } // namespace fuse_loss diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp index 832246091..571cbfd5f 100644 --- a/fuse_loss/src/dcs_loss.cpp +++ b/fuse_loss/src/dcs_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -DCSLoss::DCSLoss(const double a) -: a_(a) +DCSLoss::DCSLoss(const double a) : a_(a) { } void DCSLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void DCSLoss::print(std::ostream & stream) const +void DCSLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * DCSLoss::lossFunction() const +ceres::LossFunction* DCSLoss::lossFunction() const { return new ceres::DCSLoss(a_); } diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp index 19d3cc927..cc40cab7f 100644 --- a/fuse_loss/src/fair_loss.cpp +++ b/fuse_loss/src/fair_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -FairLoss::FairLoss(const double a) -: a_(a) +FairLoss::FairLoss(const double a) : a_(a) { } void FairLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void FairLoss::print(std::ostream & stream) const +void FairLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * FairLoss::lossFunction() const +ceres::LossFunction* FairLoss::lossFunction() const { return new ceres::FairLoss(a_); } diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp index fbe43c052..ce4ebd814 100644 --- a/fuse_loss/src/geman_mcclure_loss.cpp +++ b/fuse_loss/src/geman_mcclure_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -GemanMcClureLoss::GemanMcClureLoss(const double a) -: a_(a) +GemanMcClureLoss::GemanMcClureLoss(const double a) : a_(a) { } void GemanMcClureLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void GemanMcClureLoss::print(std::ostream & stream) const +void GemanMcClureLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * GemanMcClureLoss::lossFunction() const +ceres::LossFunction* GemanMcClureLoss::lossFunction() const { return new ceres::GemanMcClureLoss(a_); } diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp index e22cf01c8..a9b0b2d73 100644 --- a/fuse_loss/src/huber_loss.cpp +++ b/fuse_loss/src/huber_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -HuberLoss::HuberLoss(const double a) -: a_(a) +HuberLoss::HuberLoss(const double a) : a_(a) { } void HuberLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void HuberLoss::print(std::ostream & stream) const +void HuberLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * HuberLoss::lossFunction() const +ceres::LossFunction* HuberLoss::lossFunction() const { return new ceres::HuberLoss(a_); } diff --git a/fuse_loss/src/loss_function.cpp b/fuse_loss/src/loss_function.cpp index 8a64004f7..3e5c9c892 100644 --- a/fuse_loss/src/loss_function.cpp +++ b/fuse_loss/src/loss_function.cpp @@ -41,7 +41,8 @@ namespace ceres void DCSLoss::Evaluate(double s, double rho[3]) const { - if (s > a_) { + if (s > a_) + { // Outlier region const double inv = 1.0 / (a_ + s); const double scale = 2.0 * a_ * inv; @@ -49,7 +50,9 @@ void DCSLoss::Evaluate(double s, double rho[3]) const rho[0] = a_ * (3.0 * s - a_) * inv; rho[1] = scale * scale; rho[2] = -2.0 * inv * rho[1]; - } else { + } + else + { // Inlier region rho[0] = s; rho[1] = 1.0; diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp index abf9bb1a5..840a9c7ee 100644 --- a/fuse_loss/src/scaled_loss.cpp +++ b/fuse_loss/src/scaled_loss.cpp @@ -43,34 +43,32 @@ namespace fuse_loss { -ScaledLoss::ScaledLoss(const double a, const std::shared_ptr & loss) -: a_(a), loss_(loss) +ScaledLoss::ScaledLoss(const double a, const std::shared_ptr& loss) : a_(a), loss_(loss) { } void ScaledLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); loss_ = fuse_core::loadLossConfig(interfaces, name + ".loss"); } -void ScaledLoss::print(std::ostream & stream) const +void ScaledLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; - if (loss_) { + if (loss_) + { stream << " loss: " << loss_ << "\n"; } } -ceres::LossFunction * ScaledLoss::lossFunction() const +ceres::LossFunction* ScaledLoss::lossFunction() const { return new ceres::ScaledLoss(loss_ ? loss_->lossFunction() : nullptr, a_, Ownership); } diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp index 711da1df6..3b9c87d1f 100644 --- a/fuse_loss/src/softlone_loss.cpp +++ b/fuse_loss/src/softlone_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -SoftLOneLoss::SoftLOneLoss(const double a) -: a_(a) +SoftLOneLoss::SoftLOneLoss(const double a) : a_(a) { } void SoftLOneLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void SoftLOneLoss::print(std::ostream & stream) const +void SoftLOneLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * SoftLOneLoss::lossFunction() const +ceres::LossFunction* SoftLOneLoss::lossFunction() const { return new ceres::SoftLOneLoss(a_); } diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp index 06699a7c0..c46eb4cbb 100644 --- a/fuse_loss/src/tolerant_loss.cpp +++ b/fuse_loss/src/tolerant_loss.cpp @@ -42,31 +42,28 @@ namespace fuse_loss { -TolerantLoss::TolerantLoss(const double a, const double b) -: a_(a), b_(b) +TolerantLoss::TolerantLoss(const double a, const double b) : a_(a), b_(b) { } void TolerantLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); b_ = fuse_core::getParam(interfaces, name + ".b", b_); } -void TolerantLoss::print(std::ostream & stream) const +void TolerantLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n" << " b: " << b_ << "\n"; } -ceres::LossFunction * TolerantLoss::lossFunction() const +ceres::LossFunction* TolerantLoss::lossFunction() const { return new ceres::TolerantLoss(a_, b_); } diff --git a/fuse_loss/src/trivial_loss.cpp b/fuse_loss/src/trivial_loss.cpp index 283a3acda..df344edc5 100644 --- a/fuse_loss/src/trivial_loss.cpp +++ b/fuse_loss/src/trivial_loss.cpp @@ -41,12 +41,12 @@ namespace fuse_loss { -void TrivialLoss::print(std::ostream & stream) const +void TrivialLoss::print(std::ostream& stream) const { stream << type() << "\n"; } -ceres::LossFunction * TrivialLoss::lossFunction() const +ceres::LossFunction* TrivialLoss::lossFunction() const { return new ceres::TrivialLoss(); } diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp index 24ad5c657..1570e89f8 100644 --- a/fuse_loss/src/tukey_loss.cpp +++ b/fuse_loss/src/tukey_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -TukeyLoss::TukeyLoss(const double a) -: a_(a) +TukeyLoss::TukeyLoss(const double a) : a_(a) { } void TukeyLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void TukeyLoss::print(std::ostream & stream) const +void TukeyLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * TukeyLoss::lossFunction() const +ceres::LossFunction* TukeyLoss::lossFunction() const { #if CERES_VERSION_AT_LEAST(2, 0, 0) return new ceres::TukeyLoss(a_); diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp index fd409eb91..bc913fd86 100644 --- a/fuse_loss/src/welsch_loss.cpp +++ b/fuse_loss/src/welsch_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -WelschLoss::WelschLoss(const double a) -: a_(a) +WelschLoss::WelschLoss(const double a) : a_(a) { } void WelschLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void WelschLoss::print(std::ostream & stream) const +void WelschLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * WelschLoss::lossFunction() const +ceres::LossFunction* WelschLoss::lossFunction() const { return new ceres::WelschLoss(a_); } diff --git a/fuse_loss/test/test_composed_loss.cpp b/fuse_loss/test/test_composed_loss.cpp index 9ef9681a7..de607895d 100644 --- a/fuse_loss/test/test_composed_loss.cpp +++ b/fuse_loss/test/test_composed_loss.cpp @@ -59,7 +59,7 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, composed_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); EXPECT_EQ(s, rho[0]); @@ -69,7 +69,7 @@ TEST(ComposedLoss, Constructor) // Create a loss with f_loss parameter only { - std::shared_ptr f_loss{new fuse_loss::HuberLoss}; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss }; fuse_loss::ComposedLoss composed_loss(f_loss); EXPECT_NE(nullptr, composed_loss.fLoss().get()); @@ -85,10 +85,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, f_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double f_rho[3] = {0.0}; + double f_rho[3] = { 0.0 }; f_loss_function->Evaluate(s, f_rho); // Make sure 'f(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -96,14 +96,15 @@ TEST(ComposedLoss, Constructor) // Check that 'f(g(s)) == f(s)' and the same for the first and second derivatives, since g is // the TrivialLoss - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(f_rho[i], rho[i]); } } // Create a loss with g_loss parameter only { - std::shared_ptr g_loss{new fuse_loss::HuberLoss}; + std::shared_ptr g_loss{ new fuse_loss::HuberLoss }; fuse_loss::ComposedLoss composed_loss(nullptr, g_loss); EXPECT_EQ(nullptr, composed_loss.fLoss()); @@ -119,10 +120,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, g_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double g_rho[3] = {0.0}; + double g_rho[3] = { 0.0 }; g_loss_function->Evaluate(s, g_rho); // Make sure 'g(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -130,15 +131,16 @@ TEST(ComposedLoss, Constructor) // Check that 'f(g(s)) == g(s)' and the same for the first and second derivatives, since f is // the TrivialLoss - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(g_rho[i], rho[i]); } } // Create a loss with f_loss and g_loss parameters { - std::shared_ptr f_loss{new fuse_loss::HuberLoss}; - std::shared_ptr g_loss{new fuse_loss::TolerantLoss}; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss }; + std::shared_ptr g_loss{ new fuse_loss::TolerantLoss }; fuse_loss::ComposedLoss composed_loss(f_loss, g_loss); EXPECT_NE(nullptr, composed_loss.fLoss().get()); @@ -158,16 +160,16 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, g_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double g_rho[3] = {0.0}; + double g_rho[3] = { 0.0 }; g_loss_function->Evaluate(s, g_rho); // Make sure 'g(s) != s', i.e. it is not an inlier, which would be a trivial case ASSERT_NE(s, g_rho[0]); - double f_rho[3] = {0.0}; + double f_rho[3] = { 0.0 }; f_loss_function->Evaluate(g_rho[0], f_rho); // Make sure 'f(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -183,39 +185,40 @@ TEST(ComposedLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - 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(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(ComposedLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create an 'f' loss - const double a{0.05}; - std::shared_ptr f_loss{new fuse_loss::HuberLoss(a)}; + const double a{ 0.05 }; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss(a) }; // Create an 'g' loss - const double scaled_a{0.5}; - std::shared_ptr g_loss{new fuse_loss::ScaledLoss(scaled_a)}; + const double scaled_a{ 0.5 }; + std::shared_ptr g_loss{ new fuse_loss::ScaledLoss(scaled_a) }; // Create a composed loss, which illustrates the case of scaling the residuals by a factor with a // fuse_loss::ScaledLoss in the 'g' loss and applies a fuse_loss::HuberLoss loss function robust @@ -229,21 +232,19 @@ TEST(ComposedLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - composed_loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + composed_loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - composed_loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, composed_loss.lossFunction(), &x); } // Run the solver @@ -272,13 +273,13 @@ TEST(ComposedLoss, Optimization) TEST(ComposedLoss, Serialization) { // Construct an 'f' loss - const double f_loss_a{0.3}; - std::shared_ptr f_loss{new fuse_loss::HuberLoss(f_loss_a)}; + const double f_loss_a{ 0.3 }; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss(f_loss_a) }; // Construct a 'g' loss - const double g_loss_a{0.3}; - const double g_loss_b{0.6}; - std::shared_ptr g_loss{new fuse_loss::TolerantLoss(g_loss_a, g_loss_b)}; + const double g_loss_a{ 0.3 }; + const double g_loss_b{ 0.6 }; + std::shared_ptr g_loss{ new fuse_loss::TolerantLoss(g_loss_a, g_loss_b) }; // Construct a composed loss fuse_loss::ComposedLoss expected(f_loss, g_loss); @@ -307,13 +308,14 @@ TEST(ComposedLoss, Serialization) // Test inlier (s <= g_loss_a*g_loss_a) const double s = 0.95 * g_loss_a * g_loss_a; - double expected_rho[3] = {0.0}; + double expected_rho[3] = { 0.0 }; expected_loss_function->Evaluate(s, expected_rho); - double actual_rho[3] = {0.0}; + double actual_rho[3] = { 0.0 }; actual_loss_function->Evaluate(s, actual_rho); - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(expected_rho[i], actual_rho[i]); } @@ -323,7 +325,8 @@ TEST(ComposedLoss, Serialization) expected_loss_function->Evaluate(s_outlier, expected_rho); actual_loss_function->Evaluate(s_outlier, actual_rho); - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(expected_rho[i], actual_rho[i]); } } diff --git a/fuse_loss/test/test_huber_loss.cpp b/fuse_loss/test/test_huber_loss.cpp index 2884ef3b0..f21ac62d7 100644 --- a/fuse_loss/test/test_huber_loss.cpp +++ b/fuse_loss/test/test_huber_loss.cpp @@ -49,7 +49,7 @@ TEST(HuberLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::HuberLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -57,31 +57,32 @@ TEST(HuberLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - 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(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(HuberLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss fuse_loss::HuberLoss loss(0.1); @@ -92,21 +93,19 @@ TEST(HuberLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, loss.lossFunction(), &x); } // Run the solver @@ -135,7 +134,7 @@ TEST(HuberLoss, Optimization) TEST(HuberLoss, Serialization) { // Construct a loss - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::HuberLoss expected(a); // Serialize the loss into an archive @@ -158,7 +157,7 @@ TEST(HuberLoss, Serialization) // Test inlier (s <= a*a) const double s = 0.95 * a * a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_EQ(s, rho[0]); diff --git a/fuse_loss/test/test_loss_function.cpp b/fuse_loss/test/test_loss_function.cpp index 7a8e02520..005177faa 100644 --- a/fuse_loss/test/test_loss_function.cpp +++ b/fuse_loss/test/test_loss_function.cpp @@ -64,7 +64,7 @@ namespace // Compares the values of rho'(s) and rho''(s) computed by the // callback with estimates obtained by symmetric finite differencing // of rho(s). -void AssertLossFunctionIsValid(const ceres::LossFunction & loss, double s) +void AssertLossFunctionIsValid(const ceres::LossFunction& loss, double s) { ASSERT_GT(s, 0); diff --git a/fuse_loss/test/test_qwt_loss_plot.cpp b/fuse_loss/test/test_qwt_loss_plot.cpp index 2a563d1b0..21d67d7b5 100644 --- a/fuse_loss/test/test_qwt_loss_plot.cpp +++ b/fuse_loss/test/test_qwt_loss_plot.cpp @@ -57,15 +57,16 @@ class QwtLossPlotTest : public testing::Test QwtLossPlotTest() { // Generate samples: - const double step{0.01}; - const size_t half_samples{1000}; + const double step{ 0.01 }; + const size_t half_samples{ 1000 }; const double x_min = -(half_samples * step); - const size_t samples{2 * half_samples + 1}; + const size_t samples{ 2 * half_samples + 1 }; residuals.reserve(samples); - for (size_t i = 0; i < samples; ++i) { + for (size_t i = 0; i < samples; ++i) + { residuals.push_back(x_min + i * step); } } @@ -76,19 +77,15 @@ class QwtLossPlotTest : public testing::Test TEST_F(QwtLossPlotTest, PlotLossQt) { // Create losses - std::vector> losses{{ // NOLINT(whitespace/braces) - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared() - }}; + std::vector> losses{ + { // NOLINT(whitespace/braces) + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared() } + }; // Create a Qt application: int argc = 0; @@ -102,14 +99,15 @@ TEST_F(QwtLossPlotTest, PlotLossQt) fuse_loss::HSVColormap colormap(losses.size()); fuse_loss::QwtLossPlot rho_loss_plot(residuals, colormap); - auto & plot = rho_loss_plot.plot(); + auto& plot = rho_loss_plot.plot(); plot.setTitle("rho function"); plot.setAxisTitle(QwtPlot::xBottom, "r"); plot.setAxisTitle(QwtPlot::yLeft, "rho(r)"); plot.setAxisScale(QwtPlot::yLeft, 0.0, 15.0); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { rho_loss_plot.plotRho(loss); } @@ -118,7 +116,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the influence function: fuse_loss::QwtLossPlot influence_loss_plot(residuals, colormap); - auto & influence_plot = influence_loss_plot.plot(); + auto& influence_plot = influence_loss_plot.plot(); influence_plot.setTitle("influence"); influence_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -126,7 +124,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) influence_plot.setAxisScale(QwtPlot::yLeft, -3.0, 3.0); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { influence_loss_plot.plotInfluence(loss); } @@ -135,7 +134,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the weight function: fuse_loss::QwtLossPlot weight_loss_plot(residuals, colormap); - auto & weight_plot = weight_loss_plot.plot(); + auto& weight_plot = weight_loss_plot.plot(); weight_plot.setTitle("weight"); weight_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -143,7 +142,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) weight_plot.setAxisScale(QwtPlot::yLeft, 0.0, 1.5); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { weight_loss_plot.plotWeight(loss); } @@ -152,7 +152,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the second derivative function: fuse_loss::QwtLossPlot second_derivative_loss_plot(residuals, colormap); - auto & second_derivative_plot = second_derivative_loss_plot.plot(); + auto& second_derivative_plot = second_derivative_loss_plot.plot(); second_derivative_plot.setTitle("2nd derivative"); second_derivative_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -160,7 +160,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) second_derivative_plot.setAxisScale(QwtPlot::yLeft, -0.15, 0.15); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { second_derivative_loss_plot.plotSecondDerivative(loss); } diff --git a/fuse_loss/test/test_scaled_loss.cpp b/fuse_loss/test/test_scaled_loss.cpp index 7b87a9540..c732a7536 100644 --- a/fuse_loss/test/test_scaled_loss.cpp +++ b/fuse_loss/test/test_scaled_loss.cpp @@ -53,7 +53,7 @@ TEST(ScaledLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a); EXPECT_EQ(a, scaled_loss.a()); EXPECT_EQ(nullptr, scaled_loss.loss()); @@ -61,9 +61,9 @@ TEST(ScaledLoss, Constructor) // Create a loss with a parameter and loss function to scale { - std::shared_ptr loss{new fuse_loss::HuberLoss}; + std::shared_ptr loss{ new fuse_loss::HuberLoss }; - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a, loss); EXPECT_EQ(a, scaled_loss.a()); EXPECT_NE(nullptr, scaled_loss.loss()); @@ -73,38 +73,39 @@ TEST(ScaledLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - 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(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(ScaledLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss - const double a{0.1}; - std::shared_ptr loss{new fuse_loss::HuberLoss(a)}; + const double a{ 0.1 }; + std::shared_ptr loss{ new fuse_loss::HuberLoss(a) }; // Create a scaled loss, which should not have a significant impact in this test - const double scaled_a{0.7}; + const double scaled_a{ 0.7 }; fuse_loss::ScaledLoss scaled_loss(scaled_a, loss); // Build the problem. @@ -113,21 +114,19 @@ TEST(ScaledLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - scaled_loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + scaled_loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - scaled_loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, scaled_loss.lossFunction(), &x); } // Run the solver @@ -156,11 +155,11 @@ TEST(ScaledLoss, Optimization) TEST(ScaledLoss, Serialization) { // Construct a loss - const double loss_a{0.3}; - std::shared_ptr loss{new fuse_loss::HuberLoss(loss_a)}; + const double loss_a{ 0.3 }; + std::shared_ptr loss{ new fuse_loss::HuberLoss(loss_a) }; // Construct a scaled loss - const double a{0.7}; + const double a{ 0.7 }; fuse_loss::ScaledLoss expected(a, loss); // Serialize the loss into an archive @@ -184,7 +183,7 @@ TEST(ScaledLoss, Serialization) // Test inlier (s <= loss_a*loss_a) const double s = 0.95 * loss_a * loss_a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_EQ(a * s, rho[0]); diff --git a/fuse_loss/test/test_tukey_loss.cpp b/fuse_loss/test/test_tukey_loss.cpp index 39aaf6cb0..702d293a6 100644 --- a/fuse_loss/test/test_tukey_loss.cpp +++ b/fuse_loss/test/test_tukey_loss.cpp @@ -51,7 +51,7 @@ TEST(TukeyLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::TukeyLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -72,31 +72,32 @@ TEST(TukeyLoss, Evaluate) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - 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(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(TukeyLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss with a = x, so the initial value of x is not handled in the outlier region, in // which case the optimization does not convergence and the initial solution does not change @@ -108,21 +109,19 @@ TEST(TukeyLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, loss.lossFunction(), &x); } // Run the solver @@ -151,7 +150,7 @@ TEST(TukeyLoss, Optimization) TEST(TukeyLoss, Serialization) { // Construct a loss - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::TukeyLoss expected(a); // Serialize the loss into an archive @@ -174,7 +173,7 @@ TEST(TukeyLoss, Serialization) // Test inlier (s <= a*a) const double s = 0.95 * a * a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_GT(a * a / 3.0, rho[0]); diff --git a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp index f1a5b210b..ad3cc33de 100644 --- a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp @@ -45,35 +45,34 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Unicycle2DStateCostFunction() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + Unicycle2DStateCostFunction() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } } // Analytic cost function - static constexpr double dt{0.1}; + static constexpr double dt{ 0.1 }; static const fuse_core::Matrix8d sqrt_information; static const fuse_models::Unicycle2DStateCostFunction cost_function; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector8d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function process noise and covariance @@ -99,63 +98,55 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture }; // Cost function process noise and covariance -const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { - 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 -}; +const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d Unicycle2DStateCostFunction::covariance = - fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); + fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); // Parameter blocks -const double Unicycle2DStateCostFunction::position1[] = {0.0, 0.0}; -const double Unicycle2DStateCostFunction::yaw1[] = {0.0}; -const double Unicycle2DStateCostFunction::vel_linear1[] = {1.0, 0.0}; -const double Unicycle2DStateCostFunction::vel_yaw1[] = {1.570796327}; -const double Unicycle2DStateCostFunction::acc_linear1[] = {1.0, 0.0}; - -const double Unicycle2DStateCostFunction::position2[] = {0.105, 0.0}; -const double Unicycle2DStateCostFunction::yaw2[] = {0.1570796327}; -const double Unicycle2DStateCostFunction::vel_linear2[] = {1.1, 0.0}; -const double Unicycle2DStateCostFunction::vel_yaw2[] = {1.570796327}; -const double Unicycle2DStateCostFunction::acc_linear2[] = {1.0, 0.0}; +const double Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 }; +const double Unicycle2DStateCostFunction::yaw1[] = { 0.0 }; +const double Unicycle2DStateCostFunction::vel_linear1[] = { 1.0, 0.0 }; +const double Unicycle2DStateCostFunction::vel_yaw1[] = { 1.570796327 }; +const double Unicycle2DStateCostFunction::acc_linear1[] = { 1.0, 0.0 }; + +const double Unicycle2DStateCostFunction::position2[] = { 0.105, 0.0 }; +const double Unicycle2DStateCostFunction::yaw2[] = { 0.1570796327 }; +const double Unicycle2DStateCostFunction::vel_linear2[] = { 1.1, 0.0 }; +const double Unicycle2DStateCostFunction::vel_yaw2[] = { 1.570796327 }; +const double Unicycle2DStateCostFunction::acc_linear2[] = { 1.0, 0.0 }; // Analytic cost function -const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt(). - matrixU()); +const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().matrixU()); -const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{dt, - sqrt_information}; +const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{ dt, sqrt_information }; // Parameters -const double * Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, - acc_linear2 +const double* Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) + position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; -const std::vector & Unicycle2DStateCostFunction::block_sizes = - cost_function.parameter_block_sizes(); +const std::vector& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes(); const size_t Unicycle2DStateCostFunction::num_parameter_blocks = block_sizes.size(); const size_t Unicycle2DStateCostFunction::num_residuals = cost_function.num_residuals(); -BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State & state) +BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State& state) { - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_F( - Unicycle2DStateCostFunction, - AutoDiffUnicycle2DStateCostFunction)(benchmark::State & state) +BENCHMARK_F(Unicycle2DStateCostFunction, AutoDiffUnicycle2DStateCostFunction)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction< - fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2 - > - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); - for (auto _ : state) { + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } diff --git a/fuse_models/include/fuse_models/acceleration_2d.hpp b/fuse_models/include/fuse_models/acceleration_2d.hpp index c2b46b7d0..3236ccf4b 100644 --- a/fuse_models/include/fuse_models/acceleration_2d.hpp +++ b/fuse_models/include/fuse_models/acceleration_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -91,16 +90,14 @@ class Acceleration2D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for acceleration messages * @param[in] msg - The acceleration message to process */ - void process(const geometry_msgs::msg::AccelWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -125,17 +122,14 @@ class Acceleration2D : public fuse_core::AsyncSensorModel */ void onStop() override; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -146,7 +140,7 @@ class Acceleration2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; using AccelerationThrottledCallback = - fuse_core::ThrottledMessageCallback; + fuse_core::ThrottledMessageCallback; AccelerationThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b9f2b3505..b402f9dab 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -62,7 +61,7 @@ namespace common * @param[in] dimension - The erroneous dimension name * @throws runtime_error */ -inline void throwDimensionError(const std::string & dimension) +inline void throwDimensionError(const std::string& dimension) { std::string error = "Dimension " + dimension + " is not valid for this type."; RCLCPP_ERROR_STREAM(rclcpp::get_logger("fuse"), error); @@ -78,12 +77,18 @@ inline void throwDimensionError(const std::string & dimension) * @return the index of the enumerated dimension for that type * @throws runtime_error if the dimension name is invalid */ -template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +template +std::enable_if_t::value, size_t> toIndex(const std::string& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); - if (lower_dim == "x") {return static_cast(T::X);} - if (lower_dim == "y") {return static_cast(T::Y);} + if (lower_dim == "x") + { + return static_cast(T::X); + } + if (lower_dim == "y") + { + return static_cast(T::Y); + } throwDimensionError(dimension); @@ -99,11 +104,12 @@ std::enable_if_t::value, size_t> toIndex(const std::string & dim * @return the index of the enumerated dimension for that type * @throws runtime_error if the dimension name is invalid */ -template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +template +std::enable_if_t::value, size_t> toIndex(const std::string& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); - if (lower_dim == "yaw" || lower_dim == "z") { + if (lower_dim == "yaw" || lower_dim == "z") + { return static_cast(fuse_variables::Orientation2DStamped::YAW); } @@ -122,17 +128,13 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di * @return a vector of indices that are consistent with the enumerations for that variable type * @throws runtime_error if any dimension name is invalid */ -template -std::vector getDimensionIndices(const std::vector & dimension_names) +template +std::vector getDimensionIndices(const std::vector& dimension_names) { std::vector indices; indices.reserve(dimension_names.size()); - std::transform( - dimension_names.begin(), - dimension_names.end(), - std::back_inserter(indices), - toIndex); + std::transform(dimension_names.begin(), dimension_names.end(), std::back_inserter(indices), toIndex); // Remove duplicates std::sort(indices.begin(), indices.end()); diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 543dfa98f..d60b2df1f 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -71,24 +71,21 @@ #include - static auto sensor_proc_clock = rclcpp::Clock(); namespace tf2 { /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs TwistWithCovarianceStamped type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. -* \param t_in The twist to transform, as a timestamped TwistWithCovarianceStamped message. -* \param t_out The transformed twist, as a timestamped TwistWithCovarianceStamped message. -* \param transform The timestamped transform to apply, as a TransformStamped message. -*/ -template<> -inline -void doTransform( - const geometry_msgs::msg::TwistWithCovarianceStamped & t_in, - geometry_msgs::msg::TwistWithCovarianceStamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) // NOLINT + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The twist to transform, as a timestamped TwistWithCovarianceStamped message. + * \param t_out The transformed twist, as a timestamped TwistWithCovarianceStamped message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_in, + geometry_msgs::msg::TwistWithCovarianceStamped& t_out, + const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 vl; fromMsg(t_in.twist.twist.linear, vl); @@ -106,17 +103,15 @@ void doTransform( } /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs AccelWithCovarianceStamped type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. -* \param t_in The acceleration to transform, as a timestamped AccelWithCovarianceStamped message. -* \param t_out The transformed acceleration, as a timestamped AccelWithCovarianceStamped message. -* \param transform The timestamped transform to apply, as a TransformStamped message. -*/ -template<> -inline -void doTransform( - const geometry_msgs::msg::AccelWithCovarianceStamped & t_in, - geometry_msgs::msg::AccelWithCovarianceStamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) // NOLINT + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The acceleration to transform, as a timestamped AccelWithCovarianceStamped message. + * \param t_out The transformed acceleration, as a timestamped AccelWithCovarianceStamped message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& t_in, + geometry_msgs::msg::AccelWithCovarianceStamped& t_out, + const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 al; fromMsg(t_in.accel.accel.linear, al); @@ -135,7 +130,6 @@ void doTransform( } // namespace tf2 - namespace fuse_models { @@ -149,20 +143,14 @@ namespace common * @param[in] rhs_indices - RHS vector of indices * @param[in] rhs_offset - RHS offset to be added to the RHS vector indices (defaults to 0) */ -inline std::vector mergeIndices( - const std::vector & lhs_indices, - const std::vector & rhs_indices, - const size_t rhs_offset = 0u) +inline std::vector mergeIndices(const std::vector& lhs_indices, const std::vector& rhs_indices, + const size_t rhs_offset = 0u) { - auto merged_indices = - boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); + auto merged_indices = boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); const auto rhs_it = merged_indices.begin() + lhs_indices.size(); - std::transform( - rhs_it, - merged_indices.end(), - rhs_it, - std::bind(std::plus(), std::placeholders::_1, rhs_offset)); + std::transform(rhs_it, merged_indices.end(), rhs_it, + std::bind(std::plus(), std::placeholders::_1, rhs_offset)); return merged_indices; } @@ -178,17 +166,16 @@ inline std::vector mergeIndices( * @param[in,out] mean_partial - The partial measurement mean to which we want to append * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append */ -inline void populatePartialMeasurement( - const fuse_core::VectorXd & mean_full, - const fuse_core::MatrixXd & covariance_full, - const std::vector & indices, - fuse_core::VectorXd & mean_partial, - fuse_core::MatrixXd & covariance_partial) +inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, const fuse_core::MatrixXd& covariance_full, + const std::vector& indices, fuse_core::VectorXd& mean_partial, + fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) { + for (size_t r = 0; r < indices.size(); ++r) + { mean_partial(r) = mean_full(indices[r]); - for (size_t c = 0; c < indices.size(); ++c) { + for (size_t c = 0; c < indices.size(); ++c) + { covariance_partial(r, c) = covariance_full(indices[r], indices[c]); } } @@ -202,25 +189,25 @@ inline void populatePartialMeasurement( * @param[in] covariance_partial - The partial measurement covariance we want to validate * @param[in] precision - The precision to validate the partial measurements covariance is symmetric */ -inline void validatePartialMeasurement( - const fuse_core::VectorXd & mean_partial, - const fuse_core::MatrixXd & covariance_partial, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validatePartialMeasurement(const fuse_core::VectorXd& mean_partial, + const fuse_core::MatrixXd& covariance_partial, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!mean_partial.allFinite()) { + if (!mean_partial.allFinite()) + { throw std::runtime_error("Invalid partial mean " + fuse_core::to_string(mean_partial)); } - if (!fuse_core::isSymmetric(covariance_partial, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance_partial, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance_partial)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance_partial)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } } @@ -233,31 +220,30 @@ inline void validatePartialMeasurement( * @param [in] timeout - Optional. The maximum time to wait for a transform to become available. * @return true if the transform succeeded, false otherwise */ -template -bool transformMessage( - const tf2_ros::Buffer & tf_buffer, - const T & input, - T & output, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +template +bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& output, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { - try { + try + { auto trans = geometry_msgs::msg::TransformStamped(); - if (tf_timeout.nanoseconds() == 0) { - trans = tf_buffer.lookupTransform( - output.header.frame_id, input.header.frame_id, - input.header.stamp); - } else { - trans = tf_buffer.lookupTransform( - output.header.frame_id, input.header.frame_id, - input.header.stamp, tf_timeout); + if (tf_timeout.nanoseconds() == 0) + { + trans = tf_buffer.lookupTransform(output.header.frame_id, input.header.frame_id, input.header.stamp); + } + else + { + trans = tf_buffer.lookupTransform(output.header.frame_id, input.header.frame_id, input.header.stamp, tf_timeout); } tf2::doTransform(input, output, trans); return true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, - "Could not transform message from " << input.header.frame_id << " to " - << output.header.frame_id << ". Error was " << ex.what()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, + "Could not transform message from " << input.header.frame_id << " to " + << output.header.frame_id + << ". Error was " << ex.what()); } return false; @@ -284,34 +270,35 @@ bool transformMessage( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAbsolutePoseWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const fuse_core::Loss::SharedPtr & loss, - const std::string & target_frame, - const std::vector & position_indices, - const std::vector & orientation_indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAbsolutePoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, + const std::vector& position_indices, + const std::vector& orientation_indices, + const tf2_ros::Buffer& tf_buffer, const bool validate, + fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = pose; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Failed to transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } @@ -322,8 +309,7 @@ inline bool processAbsolutePoseWithCovariance( // Create the pose variable auto position = fuse_variables::Position2DStamped::make_shared(pose.header.stamp, device_id); - auto orientation = - fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); + auto orientation = fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); position->x() = absolute_pose_2d.x(); position->y() = absolute_pose_2d.y(); orientation->yaw() = absolute_pose_2d.yaw(); @@ -334,16 +320,11 @@ inline bool processAbsolutePoseWithCovariance( // Create the covariance for the constraint fuse_core::Matrix3d pose_covariance; - pose_covariance << - transformed_message.pose.covariance[0], - transformed_message.pose.covariance[1], - transformed_message.pose.covariance[5], - transformed_message.pose.covariance[6], - transformed_message.pose.covariance[7], - transformed_message.pose.covariance[11], - transformed_message.pose.covariance[30], - transformed_message.pose.covariance[31], - transformed_message.pose.covariance[35]; + pose_covariance << transformed_message.pose.covariance[0], transformed_message.pose.covariance[1], + transformed_message.pose.covariance[5], transformed_message.pose.covariance[6], + transformed_message.pose.covariance[7], transformed_message.pose.covariance[11], + transformed_message.pose.covariance[30], transformed_message.pose.covariance[31], + transformed_message.pose.covariance[35]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); @@ -351,31 +332,28 @@ inline bool processAbsolutePoseWithCovariance( const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); - populatePartialMeasurement( - pose_mean, pose_covariance, indices, pose_mean_partial, - pose_covariance_partial); + populatePartialMeasurement(pose_mean, pose_covariance, indices, pose_mean_partial, pose_covariance_partial); - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial absolute pose measurement from '" << source - << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); return false; } } // Create an absolute pose constraint - auto constraint = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - source, - *position, - *orientation, - pose_mean_partial, - pose_covariance_partial, - position_indices, - orientation_indices); + auto constraint = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared(source, *position, *orientation, pose_mean_partial, + pose_covariance_partial, position_indices, + orientation_indices); constraint->loss(loss); @@ -418,20 +396,18 @@ inline bool processAbsolutePoseWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, - const bool independent, - const fuse_core::Matrix3d & minimum_pose_relative_covariance, - const fuse_core::Loss::SharedPtr & loss, - const std::vector & position_indices, - const std::vector & orientation_indices, - const bool validate, - fuse_core::Transaction & transaction) +inline bool processDifferentialPoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, + const bool independent, + const fuse_core::Matrix3d& minimum_pose_relative_covariance, + const fuse_core::Loss::SharedPtr& loss, + const std::vector& position_indices, + const std::vector& orientation_indices, const bool validate, + fuse_core::Transaction& transaction) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } @@ -444,16 +420,13 @@ inline bool processDifferentialPoseWithCovariance( // Create the pose variables auto position1 = fuse_variables::Position2DStamped::make_shared(pose1.header.stamp, device_id); - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); orientation1->yaw() = pose1_2d.yaw(); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( - pose2.header.stamp, - device_id); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); orientation2->yaw() = pose2_2d.yaw(); @@ -464,58 +437,38 @@ inline bool processDifferentialPoseWithCovariance( double x_diff = pose2_2d.x() - pose1_2d.x(); double y_diff = pose2_2d.y() - pose1_2d.y(); fuse_core::VectorXd pose_relative_mean(3); - pose_relative_mean << - cy * x_diff - sy * y_diff, - sy * x_diff + cy * y_diff, - (pose2_2d.rotation() - pose1_2d.rotation()).getAngle(); + pose_relative_mean << cy * x_diff - sy * y_diff, sy * x_diff + cy * y_diff, + (pose2_2d.rotation() - pose1_2d.rotation()).getAngle(); // Create the covariance components for the constraint fuse_core::Matrix3d cov1; - cov1 << - pose1.pose.covariance[0], - pose1.pose.covariance[1], - pose1.pose.covariance[5], - pose1.pose.covariance[6], - pose1.pose.covariance[7], - pose1.pose.covariance[11], - pose1.pose.covariance[30], - pose1.pose.covariance[31], - pose1.pose.covariance[35]; + cov1 << pose1.pose.covariance[0], pose1.pose.covariance[1], pose1.pose.covariance[5], pose1.pose.covariance[6], + pose1.pose.covariance[7], pose1.pose.covariance[11], pose1.pose.covariance[30], pose1.pose.covariance[31], + pose1.pose.covariance[35]; fuse_core::Matrix3d cov2; - cov2 << - pose2.pose.covariance[0], - pose2.pose.covariance[1], - pose2.pose.covariance[5], - pose2.pose.covariance[6], - pose2.pose.covariance[7], - pose2.pose.covariance[11], - pose2.pose.covariance[30], - pose2.pose.covariance[31], - pose2.pose.covariance[35]; + cov2 << pose2.pose.covariance[0], pose2.pose.covariance[1], pose2.pose.covariance[5], pose2.pose.covariance[6], + pose2.pose.covariance[7], pose2.pose.covariance[11], pose2.pose.covariance[30], pose2.pose.covariance[31], + pose2.pose.covariance[35]; fuse_core::Matrix3d pose_relative_covariance; - if (independent) { + if (independent) + { // Compute Jacobians so we can rotate the covariance fuse_core::Matrix3d j_pose1; /* *INDENT-OFF* */ - j_pose1 << - -cy, sy, sy * x_diff + cy * y_diff, - -sy, -cy, -cy * x_diff + sy * y_diff, - 0, 0, -1; + j_pose1 << -cy, sy, sy * x_diff + cy * y_diff, -sy, -cy, -cy * x_diff + sy * y_diff, 0, 0, -1; /* *INDENT-ON* */ fuse_core::Matrix3d j_pose2; /* *INDENT-OFF* */ - j_pose2 << - cy, -sy, 0, - sy, cy, 0, - 0, 0, 1; + j_pose2 << cy, -sy, 0, sy, cy, 0, 0, 0, 1; /* *INDENT-ON* */ - pose_relative_covariance = j_pose1 * cov1 * j_pose1.transpose() + j_pose2 * cov2 * - j_pose2.transpose(); - } else { + pose_relative_covariance = j_pose1 * cov1 * j_pose1.transpose() + j_pose2 * cov2 * j_pose2.transpose(); + } + else + { // For dependent pose measurements p1 and p2, we assume they're computed as: // // p2 = p1 * p12 [1] @@ -679,63 +632,48 @@ inline bool processDifferentialPoseWithCovariance( // added to [2]. fuse_core::Matrix3d j_pose1; /* *INDENT-OFF* */ - j_pose1 << 1, 0, sy * pose_relative_mean(0) - cy * pose_relative_mean(1), - 0, 1, cy * pose_relative_mean(0) + sy * pose_relative_mean(1), - 0, 0, 1; + j_pose1 << 1, 0, sy * pose_relative_mean(0) - cy * pose_relative_mean(1), 0, 1, + cy * pose_relative_mean(0) + sy * pose_relative_mean(1), 0, 0, 1; /* *INDENT-ON* */ fuse_core::Matrix3d j_pose12_inv; /* *INDENT-OFF* */ - j_pose12_inv << cy, -sy, 0, - sy, cy, 0, - 0, 0, 1; + j_pose12_inv << cy, -sy, 0, sy, cy, 0, 0, 0, 1; /* *INDENT-ON* */ - pose_relative_covariance = j_pose12_inv * (cov2 - j_pose1 * cov1 * j_pose1.transpose()) * - j_pose12_inv.transpose() + - minimum_pose_relative_covariance; + pose_relative_covariance = j_pose12_inv * (cov2 - j_pose1 * cov1 * j_pose1.transpose()) * j_pose12_inv.transpose() + + minimum_pose_relative_covariance; } // Build the sub-vector and sub-matrices based on the requested indices - fuse_core::VectorXd pose_relative_mean_partial( - position_indices.size() + orientation_indices.size()); + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), - pose_relative_mean_partial.rows()); + pose_relative_mean_partial.rows()); const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); - populatePartialMeasurement( - pose_relative_mean, - pose_relative_covariance, - indices, - pose_relative_mean_partial, - pose_relative_covariance_partial); - - if (validate) { - try { - validatePartialMeasurement( - pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-6); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial differential pose measurement from '" - << source << "' source: " << ex.what()); + populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" << source + << "' source: " << ex.what()); return false; } } // Create a relative pose constraint. auto constraint = fuse_constraints::RelativePose2DStampedConstraint::make_shared( - source, - *position1, - *orientation1, - *position2, - *orientation2, - pose_relative_mean_partial, - pose_relative_covariance_partial, - position_indices, - orientation_indices); + source, *position1, *orientation1, *position2, *orientation2, pose_relative_mean_partial, + pose_relative_covariance_partial, position_indices, orientation_indices); constraint->loss(loss); @@ -782,21 +720,19 @@ inline bool processDifferentialPoseWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithTwistCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const fuse_core::Matrix3d & minimum_pose_relative_covariance, - const fuse_core::Matrix3d & twist_covariance_offset, - const fuse_core::Loss::SharedPtr & loss, - const std::vector & position_indices, - const std::vector & orientation_indices, - const bool validate, - fuse_core::Transaction & transaction) +inline bool processDifferentialPoseWithTwistCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, + const fuse_core::Matrix3d& minimum_pose_relative_covariance, + const fuse_core::Matrix3d& twist_covariance_offset, + const fuse_core::Loss::SharedPtr& loss, + const std::vector& position_indices, + const std::vector& orientation_indices, + const bool validate, fuse_core::Transaction& transaction) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } @@ -809,16 +745,13 @@ inline bool processDifferentialPoseWithTwistCovariance( // Create the pose variables auto position1 = fuse_variables::Position2DStamped::make_shared(pose1.header.stamp, device_id); - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); orientation1->yaw() = pose1_2d.yaw(); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( - pose2.header.stamp, - device_id); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); orientation2->yaw() = pose2_2d.yaw(); @@ -830,16 +763,9 @@ inline bool processDifferentialPoseWithTwistCovariance( // Create the covariance components for the constraint fuse_core::Matrix3d cov; - cov << - twist.twist.covariance[0], - twist.twist.covariance[1], - twist.twist.covariance[5], - twist.twist.covariance[6], - twist.twist.covariance[7], - twist.twist.covariance[11], - twist.twist.covariance[30], - twist.twist.covariance[31], - twist.twist.covariance[35]; + cov << twist.twist.covariance[0], twist.twist.covariance[1], twist.twist.covariance[5], twist.twist.covariance[6], + twist.twist.covariance[7], twist.twist.covariance[11], twist.twist.covariance[30], twist.twist.covariance[31], + twist.twist.covariance[35]; // For dependent pose measurements p1 and p2, we assume they're computed as: // @@ -878,10 +804,10 @@ inline bool processDifferentialPoseWithTwistCovariance( // covariance offset added to it by the publisher, so we have to remove it before using it. const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); - if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Very small time difference " << dt << "s from '" << source << "' source."); + if (dt < 1e-6) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -890,49 +816,37 @@ inline bool processDifferentialPoseWithTwistCovariance( j_twist *= dt; fuse_core::Matrix3d pose_relative_covariance = - j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + - minimum_pose_relative_covariance; + j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + minimum_pose_relative_covariance; // Build the sub-vector and sub-matrices based on the requested indices - fuse_core::VectorXd pose_relative_mean_partial( - position_indices.size() + orientation_indices.size()); + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), - pose_relative_mean_partial.rows()); + pose_relative_mean_partial.rows()); const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); - populatePartialMeasurement( - pose_relative_mean, - pose_relative_covariance, - indices, - pose_relative_mean_partial, - pose_relative_covariance_partial); - - if (validate) { - try { - validatePartialMeasurement( - pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-6); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial differential pose measurement using the twist covariance from '" - << source << "' source: " << ex.what()); + populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); return false; } } // Create a relative pose constraint. auto constraint = fuse_constraints::RelativePose2DStampedConstraint::make_shared( - source, - *position1, - *orientation1, - *position2, - *orientation2, - pose_relative_mean_partial, - pose_relative_covariance_partial, - position_indices, - orientation_indices); + source, *position1, *orientation1, *position2, *orientation2, pose_relative_mean_partial, + pose_relative_covariance_partial, position_indices, orientation_indices); constraint->loss(loss); @@ -972,36 +886,36 @@ inline bool processDifferentialPoseWithTwistCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwistWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const fuse_core::Loss::SharedPtr & linear_velocity_loss, - const fuse_core::Loss::SharedPtr & angular_velocity_loss, - const std::string & target_frame, - const std::vector & linear_indices, - const std::vector & angular_indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processTwistWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, + const fuse_core::Loss::SharedPtr& linear_velocity_loss, + const fuse_core::Loss::SharedPtr& angular_velocity_loss, + const std::string& target_frame, const std::vector& linear_indices, + const std::vector& angular_indices, const tf2_ros::Buffer& tf_buffer, + const bool validate, fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do - if (linear_indices.empty() && angular_indices.empty()) { + if (linear_indices.empty() && angular_indices.empty()) + { return false; } geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = twist; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Failed to transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } @@ -1009,56 +923,49 @@ inline bool processTwistWithCovariance( bool constraints_added = false; // Create two absolute constraints - if (!linear_indices.empty()) { - auto velocity_linear = - fuse_variables::VelocityLinear2DStamped::make_shared(twist.header.stamp, device_id); + if (!linear_indices.empty()) + { + auto velocity_linear = fuse_variables::VelocityLinear2DStamped::make_shared(twist.header.stamp, device_id); velocity_linear->x() = transformed_message.twist.twist.linear.x; velocity_linear->y() = transformed_message.twist.twist.linear.y; // Create the mean twist vectors for the constraints fuse_core::VectorXd linear_vel_mean(2); - linear_vel_mean << transformed_message.twist.twist.linear.x, - transformed_message.twist.twist.linear.y; + linear_vel_mean << transformed_message.twist.twist.linear.x, transformed_message.twist.twist.linear.y; // Create the covariances for the constraints fuse_core::MatrixXd linear_vel_covariance(2, 2); - linear_vel_covariance << - transformed_message.twist.covariance[0], - transformed_message.twist.covariance[1], - transformed_message.twist.covariance[6], - transformed_message.twist.covariance[7]; + linear_vel_covariance << transformed_message.twist.covariance[0], transformed_message.twist.covariance[1], + transformed_message.twist.covariance[6], transformed_message.twist.covariance[7]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); - fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), - linear_vel_mean_partial.rows()); + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), linear_vel_mean_partial.rows()); - populatePartialMeasurement( - linear_vel_mean, - linear_vel_covariance, - linear_indices, - linear_vel_mean_partial, - linear_vel_covariance_partial); + populatePartialMeasurement(linear_vel_mean, linear_vel_covariance, linear_indices, linear_vel_mean_partial, + linear_vel_covariance_partial); bool add_constraint = true; - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial linear velocity measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear velocity measurement from '" << source + << "' source: " << ex.what()); add_constraint = false; } } - if (add_constraint) { - auto linear_vel_constraint = - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( - source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, - linear_indices); + if (add_constraint) + { + auto linear_vel_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, linear_indices); linear_vel_constraint->loss(linear_velocity_loss); @@ -1068,10 +975,10 @@ inline bool processTwistWithCovariance( } } - if (!angular_indices.empty()) { + if (!angular_indices.empty()) + { // Create the twist variables - auto velocity_angular = - fuse_variables::VelocityAngular2DStamped::make_shared(twist.header.stamp, device_id); + auto velocity_angular = fuse_variables::VelocityAngular2DStamped::make_shared(twist.header.stamp, device_id); velocity_angular->yaw() = transformed_message.twist.twist.angular.z; fuse_core::VectorXd angular_vel_vector(1); @@ -1082,22 +989,25 @@ inline bool processTwistWithCovariance( bool add_constraint = true; - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(angular_vel_vector, angular_vel_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, - "Invalid partial angular velocity measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Invalid partial angular velocity measurement from '" + << source << "' source: " << ex.what()); add_constraint = false; } } - if (add_constraint) { - auto angular_vel_constraint = - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( - source, *velocity_angular, angular_vel_vector, angular_vel_covariance, angular_indices); + if (add_constraint) + { + auto angular_vel_constraint = fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + source, *velocity_angular, angular_vel_vector, angular_vel_covariance, angular_indices); angular_vel_constraint->loss(angular_velocity_loss); @@ -1107,7 +1017,8 @@ inline bool processTwistWithCovariance( } } - if (constraints_added) { + if (constraints_added) + { transaction.addInvolvedStamp(twist.header.stamp); } @@ -1136,42 +1047,41 @@ inline bool processTwistWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAccelWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::AccelWithCovarianceStamped & acceleration, - const fuse_core::Loss::SharedPtr & loss, - const std::string & target_frame, - const std::vector & indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAccelWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::AccelWithCovarianceStamped& acceleration, + const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, + const std::vector& indices, const tf2_ros::Buffer& tf_buffer, + const bool validate, fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do - if (indices.empty()) { + if (indices.empty()) + { return false; } geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = acceleration; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, - "Failed to transform acceleration message with stamp " << - rclcpp::Time(acceleration.header.stamp).nanoseconds() - << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Failed to transform acceleration message with stamp " + << rclcpp::Time(acceleration.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } // Create the acceleration variables auto acceleration_linear = - fuse_variables::AccelerationLinear2DStamped::make_shared(acceleration.header.stamp, device_id); + fuse_variables::AccelerationLinear2DStamped::make_shared(acceleration.header.stamp, device_id); acceleration_linear->x() = transformed_message.accel.accel.linear.x; acceleration_linear->y() = transformed_message.accel.accel.linear.y; @@ -1180,41 +1090,33 @@ inline bool processAccelWithCovariance( accel_mean << transformed_message.accel.accel.linear.x, transformed_message.accel.accel.linear.y; fuse_core::MatrixXd accel_covariance(2, 2); - accel_covariance << - transformed_message.accel.covariance[0], - transformed_message.accel.covariance[1], - transformed_message.accel.covariance[6], - transformed_message.accel.covariance[7]; + accel_covariance << transformed_message.accel.covariance[0], transformed_message.accel.covariance[1], + transformed_message.accel.covariance[6], transformed_message.accel.covariance[7]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); - fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), - accel_mean_partial.rows()); + fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), accel_mean_partial.rows()); - populatePartialMeasurement( - accel_mean, accel_covariance, indices, accel_mean_partial, - accel_covariance_partial); + populatePartialMeasurement(accel_mean, accel_covariance, indices, accel_mean_partial, accel_covariance_partial); - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial linear acceleration measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); return false; } } // Create the constraint - auto linear_accel_constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - source, - *acceleration_linear, - accel_mean_partial, - accel_covariance_partial, - indices); + auto linear_accel_constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + source, *acceleration_linear, accel_mean_partial, accel_covariance_partial, indices); linear_accel_constraint->loss(loss); @@ -1235,10 +1137,9 @@ inline bool processAccelWithCovariance( * @param[in] velocity_yaw - The yaw velocity * @param[in] velocity_norm_min - The minimum velocity norm */ -inline void scaleProcessNoiseCovariance( - fuse_core::Matrix8d & process_noise_covariance, - const tf2_2d::Vector2 & velocity_linear, const double velocity_yaw, - const double velocity_norm_min) +inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covariance, + const tf2_2d::Vector2& velocity_linear, const double velocity_yaw, + const double velocity_norm_min) { // A more principled approach would be to get the current velocity from the state, make a diagonal // matrix from it, and then rotate it to be in the world frame (i.e., the same frame as the pose @@ -1258,12 +1159,10 @@ inline void scaleProcessNoiseCovariance( fuse_core::Matrix3d velocity; velocity.setIdentity(); velocity.diagonal() *= - std::max( - velocity_norm_min, - fuse_core::Vector3d(velocity_linear.x(), velocity_linear.y(), velocity_yaw).norm()); + std::max(velocity_norm_min, fuse_core::Vector3d(velocity_linear.x(), velocity_linear.y(), velocity_yaw).norm()); process_noise_covariance.topLeftCorner<3, 3>() = - velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); + velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } } // namespace common diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index b5a77de98..10006456f 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -40,50 +40,49 @@ #include #include - namespace fuse_models { namespace common { -template +template struct is_linear_2d { static const bool value = false; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template +template struct is_angular_2d { static const bool value = false; }; -template<> +template <> struct is_angular_2d { static const bool value = true; }; -template<> +template <> struct is_angular_2d { static const bool value = true; diff --git a/fuse_models/include/fuse_models/graph_ignition.hpp b/fuse_models/include/fuse_models/graph_ignition.hpp index 0f27da202..6d105ba99 100644 --- a/fuse_models/include/fuse_models/graph_ignition.hpp +++ b/fuse_models/include/fuse_models/graph_ignition.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -96,10 +95,8 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -127,15 +124,14 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - void subscriberCallback(const fuse_msgs::msg::SerializedGraph & msg); + void subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg); /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - bool setGraphServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetGraph::Request::SharedPtr req); + bool setGraphServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetGraph::Request::SharedPtr req); /** * @brief Perform any required initialization for the kinematic ignition sensor @@ -150,8 +146,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * * @param[in] msg - The graph message */ - void process( - const fuse_msgs::msg::SerializedGraph & msg, std::function post_process = nullptr); + void process(const fuse_msgs::msg::SerializedGraph& msg, std::function post_process = nullptr); /** * @brief Create and send a transaction equivalent to the supplied graph @@ -159,20 +154,16 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * @param[in] graph - The graph * @param[in] stamp - The graph stamp */ - void sendGraph(const fuse_core::Graph & graph, const rclcpp::Time & stamp); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ std::atomic_bool started_; //!< Flag indicating the sensor has been started - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/imu_2d.hpp b/fuse_models/include/fuse_models/imu_2d.hpp index 82db3b734..c95a36349 100644 --- a/fuse_models/include/fuse_models/imu_2d.hpp +++ b/fuse_models/include/fuse_models/imu_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -115,16 +114,14 @@ class Imu2D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu & msg); + void process(const sensor_msgs::msg::Imu& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -158,22 +155,18 @@ class Imu2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_2d.hpp b/fuse_models/include/fuse_models/odometry_2d.hpp index f921bcec9..8edbc5d20 100644 --- a/fuse_models/include/fuse_models/odometry_2d.hpp +++ b/fuse_models/include/fuse_models/odometry_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -106,16 +105,14 @@ class Odometry2D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry & msg); + void process(const nav_msgs::msg::Odometry& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -149,22 +146,18 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_2d_publisher.hpp b/fuse_models/include/fuse_models/odometry_2d_publisher.hpp index dc51b0713..48e128bd0 100644 --- a/fuse_models/include/fuse_models/odometry_2d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_2d_publisher.hpp @@ -56,7 +56,6 @@ #include #include - namespace fuse_models { @@ -123,9 +122,8 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; protected: /** @@ -142,9 +140,8 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Perform any required operations before the first call to notify() occurs @@ -176,17 +173,11 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, + fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, + fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration); /** * @brief Timer callback method for the filtered state publication and tf broadcasting @@ -198,31 +189,24 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * @brief Object that searches for the most recent common timestamp for a set of variables */ using Synchronizer = fuse_publishers::StampedVariableSynchronizer< - fuse_variables::Orientation2DStamped, - fuse_variables::Position2DStamped, - fuse_variables::VelocityLinear2DStamped, - fuse_variables::VelocityAngular2DStamped, - fuse_variables::AccelerationLinear2DStamped>; - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - fuse_core::UUID device_id_; //!< The UUID of this device + fuse_variables::Orientation2DStamped, fuse_variables::Position2DStamped, fuse_variables::VelocityLinear2DStamped, + fuse_variables::VelocityAngular2DStamped, fuse_variables::AccelerationLinear2DStamped>; + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger ParameterType params_; rclcpp::Time latest_stamp_; rclcpp::Time latest_covariance_stamp_; - bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not nav_msgs::msg::Odometry odom_output_; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; @@ -237,10 +221,10 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher std::shared_ptr tf_broadcaster_ = nullptr; std::unique_ptr tf_listener_; - fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to - //!< print delayed throttle - //!< messages, that can be reset - //!< on start + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{ 10.0 }; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h index c47431f98..f427cd491 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/acceleration_2d_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/acceleration_2d_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp index 3b5fc1ead..fdf6b4f64 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -62,65 +61,37 @@ struct Acceleration2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "dimensions")); + interfaces, fuse_core::joinParameterName(ns, "dimensions")); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - fuse_core::getParamRequired( - interfaces, fuse_core::joinParameterName( - ns, - "target_frame"), - target_frame); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - bool disable_checks {false}; - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string target_frame {}; + bool disable_checks{ false }; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string target_frame{}; std::vector indices; fuse_core::Loss::SharedPtr loss; }; diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h b/fuse_models/include/fuse_models/parameters/graph_ignition_params.h index ee24e5ec9..7739b4fd8 100644 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/graph_ignition_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/graph_ignition_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/graph_ignition_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp index 1452a5b58..b22826135 100644 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp @@ -39,7 +39,6 @@ #include - namespace fuse_models { @@ -59,51 +58,37 @@ struct GraphIgnitionParams : public fuse_models::parameters::ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_graph_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_graph_service"), - set_graph_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_graph_service"), set_graph_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); } /** * @brief The size of the subscriber queue for the topic */ - int queue_size{10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service{"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_graph service to advertise */ - std::string set_graph_service{"set_graph"}; + std::string set_graph_service{ "set_graph" }; /** * @brief The topic name for received SerializedGraph messages */ - std::string topic{"graph"}; + std::string topic{ "graph" }; }; } // namespace parameters diff --git a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp index c534c3cc0..f8cf24fe8 100644 --- a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -65,140 +64,81 @@ struct Imu2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - linear_acceleration_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_acceleration_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + linear_acceleration_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); fuse_core::getPositiveParam(interfaces, "tf_timeout", tf_timeout, false); fuse_core::getPositiveParam(interfaces, "throttle_period", throttle_period, false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); - - remove_gravitational_acceleration = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); - gravitational_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "gravitational_acceleration"), - gravitational_acceleration); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "remove_gravitational_acceleration"), + remove_gravitational_acceleration); + gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "gravitational_acceleration"), gravitational_acceleration); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - acceleration_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_target_frame"), - acceleration_target_frame); - orientation_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_target_frame"), - orientation_target_frame); + acceleration_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "acceleration_target_frame"), acceleration_target_frame); + orientation_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "orientation_target_frame"), orientation_target_frame); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); - - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); linear_acceleration_loss = - fuse_core::loadLossConfig( - interfaces, - fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be subtracted in order to - //!< recover the raw values - bool remove_gravitational_acceleration {false}; - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - double gravitational_acceleration {9.80665}; - std::string acceleration_target_frame {}; - std::string orientation_target_frame {}; - std::string topic {}; - std::string twist_target_frame {}; + fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be subtracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration{ false }; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration{ 9.80665 }; + std::string acceleration_target_frame{}; + std::string orientation_target_frame{}; + std::string topic{}; + std::string twist_target_frame{}; std::vector angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h index 8103cad0b..1925cc5f4 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/odometry_2d_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp index 80bfe238d..f178d8bda 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_models { @@ -66,135 +65,73 @@ struct Odometry2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { position_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - linear_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_dimensions")); - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + linear_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_velocity_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); pose_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "pose_target_frame"), - pose_target_frame); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); linear_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_velocity_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be subtracted in order to - //!< recover the raw values - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string pose_target_frame {}; - std::string twist_target_frame {}; + fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be subtracted in order to + //!< recover the raw values + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string pose_target_frame{}; + std::string twist_target_frame{}; std::vector position_indices; std::vector orientation_indices; std::vector linear_velocity_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h index 080693470..7bec22d88 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/odometry_2d_publisher_params.hpp \ +#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_publisher_params.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp index 488a38aaa..c65f9c361 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp @@ -49,7 +49,6 @@ #include - namespace fuse_models { @@ -71,168 +70,93 @@ struct Odometry2DPublisherParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - publish_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_tf"), - publish_tf); - invert_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "invert_tf"), - invert_tf); - predict_to_current_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_to_current_time"), - predict_to_current_time); - predict_with_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_with_acceleration"), - predict_with_acceleration); + publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); + invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); + predict_to_current_time = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time); + predict_with_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_with_acceleration"), predict_with_acceleration); publish_frequency = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_frequency"), - publish_frequency); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_frequency"), publish_frequency); process_noise_covariance = fuse_core::getCovarianceDiagonalParam<8>( - interfaces, fuse_core::joinParameterName( - ns, - "process_noise_diagonal"), 0.0); + interfaces, fuse_core::joinParameterName(ns, "process_noise_diagonal"), 0.0); scale_process_noise = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "scale_process_noise"), - scale_process_noise); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "scale_process_noise"), scale_process_noise); velocity_norm_min = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_norm_min"), - velocity_norm_min); - - fuse_core::getPositiveParam( - interfaces, - fuse_core::joinParameterName( - ns, - "covariance_throttle_period"), covariance_throttle_period, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_cache_time"), tf_cache_time, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - - map_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "map_frame_id"), - map_frame_id); - odom_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "odom_frame_id"), - odom_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "velocity_norm_min"), velocity_norm_min); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "covariance_throttle_period"), + covariance_throttle_period, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_cache_time"), tf_cache_time, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + + map_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "map_frame_id"), map_frame_id); + odom_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "odom_frame_id"), odom_frame_id); base_link_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_frame_id"), - base_link_frame_id); - base_link_output_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_output_frame_id"), - base_link_output_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "base_link_frame_id"), base_link_frame_id); + base_link_output_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "base_link_output_frame_id"), base_link_output_frame_id); world_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "world_frame_id"), - world_frame_id); - - const bool frames_valid = - map_frame_id != odom_frame_id && - map_frame_id != base_link_frame_id && - map_frame_id != base_link_output_frame_id && - odom_frame_id != base_link_frame_id && - odom_frame_id != base_link_output_frame_id && - (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); - - if (!frames_valid) { - RCLCPP_FATAL_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "Invalid frame configuration! Please note:\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " - << "unique\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " - << "unique\n" - << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); + + const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) + { + RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); assert(frames_valid); } topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); acceleration_topic = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_topic"), - acceleration_topic); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "acceleration_topic"), acceleration_topic); fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); } - bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not - bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When - //!< the inverse is broadcasted, the transform is inverted and the - //!< header.frame_id and child_frame_id are swapped, i.e. the odometry - //!< output header.frame_id is set to the base_link_output_frame_id and - //!< the child_frame_id to the world_frame_id - bool predict_to_current_time {false}; - bool predict_with_acceleration {false}; - double publish_frequency {10.0}; - fuse_core::Matrix8d process_noise_covariance; //!< Process noise covariance matrix - bool scale_process_noise{false}; - double velocity_norm_min{1e-3}; - rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds - //!< to compute the covariance - rclcpp::Duration tf_cache_time {10, 0}; - rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; - int queue_size {1}; - std::string map_frame_id {"map"}; - std::string odom_frame_id {"odom"}; - std::string base_link_frame_id {"base_link"}; - std::string base_link_output_frame_id {base_link_frame_id}; - std::string world_frame_id {odom_frame_id}; - std::string topic {"odometry/filtered"}; - std::string acceleration_topic {"acceleration/filtered"}; + bool publish_tf{ true }; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{ false }; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time{ false }; + bool predict_with_acceleration{ false }; + double publish_frequency{ 10.0 }; + fuse_core::Matrix8d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{ false }; + double velocity_norm_min{ 1e-3 }; + rclcpp::Duration covariance_throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time{ 10, 0 }; + rclcpp::Duration tf_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; + int queue_size{ 1 }; + std::string map_frame_id{ "map" }; + std::string odom_frame_id{ "odom" }; + std::string base_link_frame_id{ "base_link" }; + std::string base_link_output_frame_id{ base_link_frame_id }; + std::string world_frame_id{ odom_frame_id }; + std::string topic{ "odometry/filtered" }; + std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.hpp b/fuse_models/include/fuse_models/parameters/parameter_base.hpp index def42a29e..0508d9041 100644 --- a/fuse_models/include/fuse_models/parameters/parameter_base.hpp +++ b/fuse_models/include/fuse_models/parameters/parameter_base.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_models { @@ -60,12 +59,10 @@ struct ParameterBase * @param[in] ns - The parameter namespace to use */ virtual void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) = 0; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) = 0; }; /** @@ -77,14 +74,15 @@ struct ParameterBase * @param[in] name - The ROS parameter name for the sensor configuration parameter * @return A vector with the dimension indices, that would be empty if the parameter does not exist */ -template -inline std::vector loadSensorConfig( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +template +inline std::vector +loadSensorConfig(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { std::vector dimensions; dimensions = fuse_core::getParam(interfaces, name, dimensions); - if (!dimensions.empty()) { + if (!dimensions.empty()) + { return common::getDimensionIndices(dimensions); } diff --git a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp index d68b36bc3..82bbe0cc0 100644 --- a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,93 +63,55 @@ struct Pose2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { position_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - target_frame = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "target_frame"), - target_frame); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - - if (!independent) { - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + + if (!independent) + { + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); } } loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string target_frame {}; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string target_frame{}; std::vector position_indices; std::vector orientation_indices; fuse_core::Loss::SharedPtr loss; diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.h b/fuse_models/include/fuse_models/parameters/transaction_params.h index 9ed5a7583..014e45d5b 100644 --- a/fuse_models/include/fuse_models/parameters/transaction_params.h +++ b/fuse_models/include/fuse_models/parameters/transaction_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/transaction_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/transaction_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.hpp b/fuse_models/include/fuse_models/parameters/transaction_params.hpp index b025bcffc..95669e555 100644 --- a/fuse_models/include/fuse_models/parameters/transaction_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transaction_params.hpp @@ -41,7 +41,6 @@ #include - namespace fuse_models { @@ -61,22 +60,16 @@ struct TransactionParams : public fuse_models::parameters::ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); } - int queue_size{10}; + int queue_size{ 10 }; std::string topic{}; }; diff --git a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp index dc0129dd1..62061d1b7 100644 --- a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,72 +63,40 @@ struct Twist2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { linear_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "linear_dimensions")); angular_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "angular_dimensions")); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - fuse_core::getParamRequired( - interfaces, fuse_core::joinParameterName( - ns, - "target_frame"), - target_frame); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); - linear_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_loss")); - angular_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_loss")); + linear_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_loss")); + angular_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_loss")); } - bool disable_checks {false}; - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string target_frame {}; + bool disable_checks{ false }; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string target_frame{}; std::vector linear_indices; std::vector angular_indices; fuse_core::Loss::SharedPtr linear_loss; diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h index 0c4b4746f..8cba776a3 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/unicycle_2d_ignition_params.hpp \ +#warning This header is obsolete, please include fuse_models/parameters/unicycle_2d_ignition_params.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp index 8a3ca5d4f..d55023858 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,88 +63,54 @@ struct Unicycle2DIgnitionParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { publish_on_startup = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_on_startup"), - publish_on_startup); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_pose_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_service"), - set_pose_service); - set_pose_deprecated_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_deprecated_service"), - set_pose_deprecated_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_pose_service"), set_pose_service); + set_pose_deprecated_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "set_pose_deprecated_service"), set_pose_deprecated_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); std::vector sigma_vector; - sigma_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_sigma"), - sigma_vector); - if (!sigma_vector.empty()) { - if (sigma_vector.size() != 8) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must be length 8, but " - "is actually length " + - std::to_string(sigma_vector.size())); + sigma_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_sigma"), sigma_vector); + if (!sigma_vector.empty()) + { + if (sigma_vector.size() != 8) + { + throw std::invalid_argument("The supplied initial_sigma parameter must be length 8, but " + "is actually length " + + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; - if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) + { + throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } std::vector state_vector; - state_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_state"), - state_vector); - if (!state_vector.empty()) { - if (state_vector.size() != 8) { - throw std::invalid_argument( - "The supplied initial_state parameter must be length 8, but is actually length " + - std::to_string(state_vector.size())); + state_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_state"), state_vector); + if (!state_vector.empty()) + { + if (state_vector.size() != 8) + { + throw std::invalid_argument("The supplied initial_state parameter must be length 8, but is actually length " + + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; - if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument( - "The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + auto is_state_valid = [](const double state) { return std::isfinite(state); }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) + { + throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } @@ -153,37 +118,36 @@ struct Unicycle2DIgnitionParams : public ParameterBase loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - /** * @brief Flag indicating if an initial state transaction should be sent on startup, or only in * response to a set_pose service call or topic message. */ - bool publish_on_startup {true}; + bool publish_on_startup{ true }; /** * @brief The size of the subscriber queue for the set_pose topic */ - int queue_size {10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_pose service to advertise */ - std::string set_pose_service {"set_pose"}; + std::string set_pose_service{ "set_pose" }; /** * @brief The name of the deprecated set_pose service without return codes */ - std::string set_pose_deprecated_service {"set_pose_deprecated"}; + std::string set_pose_deprecated_service{ "set_pose_deprecated" }; /** * @brief The topic name for received PoseWithCovarianceStamped messages */ - std::string topic {"set_pose"}; + std::string topic{ "set_pose" }; /** * @brief The uncertainty of the initial state value @@ -193,14 +157,13 @@ struct Unicycle2DIgnitionParams : public ParameterBase * The covariance matrix is created placing the squared standard deviations along the diagonal of * an 8x8 matrix. */ - std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9}; + std::vector initial_sigma{ 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 }; /** * @brief The initial value of the 8-dimension state vector (x, y, yaw, x_vel, y_vel, yaw_vel, * x_acc, y_acc) */ - std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector initial_state{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; /** * @brief Loss function diff --git a/fuse_models/include/fuse_models/pose_2d.hpp b/fuse_models/include/fuse_models/pose_2d.hpp index 51d361276..154102025 100644 --- a/fuse_models/include/fuse_models/pose_2d.hpp +++ b/fuse_models/include/fuse_models/pose_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -96,16 +95,14 @@ class Pose2D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -137,21 +134,17 @@ class Pose2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -163,8 +156,7 @@ class Pose2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; - using PoseThrottledCallback = - fuse_core::ThrottledMessageCallback; + using PoseThrottledCallback = fuse_core::ThrottledMessageCallback; PoseThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/transaction.hpp b/fuse_models/include/fuse_models/transaction.hpp index febcf7ecf..ba2517069 100644 --- a/fuse_models/include/fuse_models/transaction.hpp +++ b/fuse_models/include/fuse_models/transaction.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -83,10 +82,8 @@ class Transaction : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; protected: /** @@ -108,15 +105,12 @@ class Transaction : public fuse_core::AsyncSensorModel * @brief Callback for transaction messages * @param[in] msg - The transaction message to process */ - void process(const fuse_msgs::msg::SerializedTransaction & msg); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void process(const fuse_msgs::msg::SerializedTransaction& msg); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/twist_2d.hpp b/fuse_models/include/fuse_models/twist_2d.hpp index 5e8004d4f..de3494c59 100644 --- a/fuse_models/include/fuse_models/twist_2d.hpp +++ b/fuse_models/include/fuse_models/twist_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -91,16 +90,14 @@ class Twist2D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for twist messages * @param[in] msg - The twist message to process */ - void process(const geometry_msgs::msg::TwistWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -120,17 +117,14 @@ class Twist2D : public fuse_core::AsyncSensorModel */ void onStop() override; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -140,8 +134,7 @@ class Twist2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; - using TwistThrottledCallback = - fuse_core::ThrottledMessageCallback; + using TwistThrottledCallback = fuse_core::ThrottledMessageCallback; TwistThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/unicycle_2d.hpp b/fuse_models/include/fuse_models/unicycle_2d.hpp index 2a4766aa2..808d9865e 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -92,11 +91,10 @@ class Unicycle2D : public fuse_core::AsyncMotionModel /** * @brief Shadowing extension to the AsyncMotionModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; protected: /** @@ -104,19 +102,19 @@ class Unicycle2D : public fuse_core::AsyncMotionModel */ struct StateHistoryElement { - fuse_core::UUID position_uuid; //!< The uuid of the associated position variable - fuse_core::UUID yaw_uuid; //!< The uuid of the associated orientation variable - fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable - fuse_core::UUID vel_yaw_uuid; //!< The uuid of the associated angular velocity variable - fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration - //!< variable + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID yaw_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_yaw_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable tf2_2d::Transform pose; //!< Map-frame pose tf2_2d::Vector2 velocity_linear; //!< Body-frame linear velocity - double velocity_yaw{0.0}; //!< Body-frame yaw velocity + double velocity_yaw{ 0.0 }; //!< Body-frame yaw velocity tf2_2d::Vector2 acceleration_linear; //!< Body-frame linear acceleration - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; /** * @brief Validate the state components: pose, linear velocity, yaw velocity and linear @@ -138,7 +136,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * constraints * @return True if the motion models were generated successfully, false otherwise */ - bool applyCallback(fuse_core::Transaction & transaction) override; + bool applyCallback(fuse_core::Transaction& transaction) override; /** * @brief Generate a single motion model segment between the specified timestamps. @@ -158,11 +156,9 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables); + void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -189,10 +185,8 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length); + static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -205,39 +199,35 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix8d & process_noise_covariance); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix8d& process_noise_covariance); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger rclcpp::Duration buffer_length_; //!< The length of the state history fuse_core::UUID device_id_; //!< The UUID of the device to be published fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created //!< motion model segments fuse_core::Matrix8d process_noise_covariance_; //!< Process noise covariance matrix - bool scale_process_noise_{false}; //!< Whether to scale the process noise + bool scale_process_noise_{ false }; //!< Whether to scale the process noise //!< covariance pose by the norm of the current //!< state twist - double velocity_norm_min_{1e-3}; //!< The minimum velocity/twist norm allowed when + double velocity_norm_min_{ 1e-3 }; //!< The minimum velocity/twist norm allowed when //!< scaling the process noise covariance - bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and - //!< predicted state, including the process noise covariance after - //!< it is scaled and multiplied by dt - StateHistory state_history_; //!< History of optimized graph pose estimates + bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Unicycle2D & unicycle_2d); +std::ostream& operator<<(std::ostream& stream, const Unicycle2D& unicycle_2d); } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp index 28b12bafe..832d25239 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -108,10 +107,8 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -138,23 +135,20 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr, - const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); protected: /** @@ -170,9 +164,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - std::function post_process = nullptr); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose @@ -183,24 +175,20 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ - - std::atomic_bool started_; //!< Flag indicating the sensor has been started - bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already - fuse_core::UUID device_id_; //!< The UUID of this device + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/unicycle_2d_predict.hpp b/fuse_models/include/fuse_models/unicycle_2d_predict.hpp index 3d90b51cd..a7d4b1cdb 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_predict.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -66,25 +65,11 @@ namespace fuse_models * @param[out] acc_linear2_x - Second X acceleration * @param[out] acc_linear2_y - Second Y acceleration */ -template -inline void predict( - const T position1_x, - const T position1_y, - const T yaw1, - const T vel_linear1_x, - const T vel_linear1_y, - const T vel_yaw1, - const T acc_linear1_x, - const T acc_linear1_y, - const T dt, - T & position2_x, - T & position2_y, - T & yaw2, - T & vel_linear2_x, - T & vel_linear2_y, - T & vel_yaw2, - T & acc_linear2_x, - T & acc_linear2_y) +template +inline void predict(const T position1_x, const T position1_y, const T yaw1, const T vel_linear1_x, + const T vel_linear1_y, const T vel_yaw1, const T acc_linear1_x, const T acc_linear1_y, const T dt, + T& position2_x, T& position2_y, T& yaw2, T& vel_linear2_x, T& vel_linear2_y, T& vel_yaw2, + T& acc_linear2_x, T& acc_linear2_y) { // There are better models for this projection, but this matches the one used by r_l. T sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model @@ -125,25 +110,11 @@ inline void predict( * @param[out] acc_linear2_y - Second Y acceleration * @param[out] jacobians - Jacobians wrt the state */ -inline void predict( - const double position1_x, - const double position1_y, - const double yaw1, - const double vel_linear1_x, - const double vel_linear1_y, - const double vel_yaw1, - const double acc_linear1_x, - const double acc_linear1_y, - const double dt, - double & position2_x, - double & position2_y, - double & yaw2, - double & vel_linear2_x, - double & vel_linear2_y, - double & vel_yaw2, - double & acc_linear2_x, - double & acc_linear2_y, - double ** jacobians) +inline void predict(const double position1_x, const double position1_y, const double yaw1, const double vel_linear1_x, + const double vel_linear1_y, const double vel_yaw1, const double acc_linear1_x, + const double acc_linear1_y, const double dt, double& position2_x, double& position2_y, double& yaw2, + double& vel_linear2_x, double& vel_linear2_y, double& vel_yaw2, double& acc_linear2_x, + double& acc_linear2_y, double** jacobians) { // There are better models for this projection, but this matches the one used by r_l. const double sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses @@ -168,62 +139,47 @@ inline void predict( fuse_core::wrapAngle2D(yaw2); - if (jacobians) { + if (jacobians) + { // Jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); - jacobian << 1, 0, - 0, 1, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0; + jacobian << 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; } // Jacobian wrt yaw1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map jacobian(jacobians[1]); jacobian << -delta_y_rot, delta_x_rot, 1, 0, 0, 0, 0, 0; } // Jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { const double cy_dt = cy * dt; const double sy_dt = sy * dt; Eigen::Map> jacobian(jacobians[2]); - jacobian << cy_dt, -sy_dt, - sy_dt, cy_dt, - 0, 0, - 1, 0, - 0, 1, - 0, 0, - 0, 0, - 0, 0; + jacobian << cy_dt, -sy_dt, sy_dt, cy_dt, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0; } // Jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map jacobian(jacobians[3]); jacobian << 0, 0, dt, 0, 0, 1, 0, 0; } // Jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { const double cy_half_dt2 = cy * half_dt2; const double sy_half_dt2 = sy * half_dt2; Eigen::Map> jacobian(jacobians[4]); - jacobian << cy_half_dt2, -sy_half_dt2, - sy_half_dt2, cy_half_dt2, - 0, 0, - dt, 0, - 0, dt, - 0, 0, - 1, 0, - 0, 1; + jacobian << cy_half_dt2, -sy_half_dt2, sy_half_dt2, cy_half_dt2, 0, 0, dt, 0, 0, dt, 0, 0, 1, 0, 0, 1; } } } @@ -242,38 +198,14 @@ inline void predict( * @param[out] vel_yaw2 - Second yaw velocity * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) */ -template -inline void predict( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T dt, - T * const position2, - T * const yaw2, - T * const vel_linear2, - T * const vel_yaw2, - T * const acc_linear2) +template +inline void predict(const T* const position1, const T* const yaw1, const T* const vel_linear1, const T* const vel_yaw1, + const T* const acc_linear1, const T dt, T* const position2, T* const yaw2, T* const vel_linear2, + T* const vel_yaw2, T* const acc_linear2) { - predict( - position1[0], - position1[1], - *yaw1, - vel_linear1[0], - vel_linear1[1], - *vel_yaw1, - acc_linear1[0], - acc_linear1[1], - dt, - position2[0], - position2[1], - *yaw2, - vel_linear2[0], - vel_linear2[1], - *vel_yaw2, - acc_linear2[0], - acc_linear2[1]); + predict(position1[0], position1[1], *yaw1, vel_linear1[0], vel_linear1[1], *vel_yaw1, acc_linear1[0], acc_linear1[1], + dt, position2[0], position2[1], *yaw2, vel_linear2[0], vel_linear2[1], *vel_yaw2, acc_linear2[0], + acc_linear2[1]); } /** @@ -289,62 +221,40 @@ inline void predict( * @param[in] acc_linear2 - The second linear acceleration * @param[in] jacobian - The jacobian wrt the state */ -inline void predict( - const tf2_2d::Transform & pose1, - const tf2_2d::Vector2 & vel_linear1, - const double vel_yaw1, - const tf2_2d::Vector2 & acc_linear1, - const double dt, - tf2_2d::Transform & pose2, - tf2_2d::Vector2 & vel_linear2, - double & vel_yaw2, - tf2_2d::Vector2 & acc_linear2, - fuse_core::Matrix8d & jacobian) +inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, + const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, + tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2, + fuse_core::Matrix8d& jacobian) { - double x_pred {}; - double y_pred {}; - double yaw_pred {}; - double vel_linear_x_pred {}; - double vel_linear_y_pred {}; - double acc_linear_x_pred {}; - double acc_linear_y_pred {}; + double x_pred{}; + double y_pred{}; + double yaw_pred{}; + double vel_linear_x_pred{}; + double vel_linear_y_pred{}; + double acc_linear_x_pred{}; + double acc_linear_y_pred{}; // fuse_core::Matrix8d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per // parameter block and later reconstruct the fuse_core::Matrix8d with the full jacobian. The // parameter blocks have the following sizes: {position1: 2, yaw1: 1, vel_linear1: 2, vel_yaw1: 1, // acc_linear1: 2} - static constexpr size_t num_residuals{8}; - static constexpr size_t num_parameter_blocks{5}; - static const std::array block_sizes = {2, 1, 2, 1, 2}; + static constexpr size_t num_residuals{ 8 }; + static constexpr size_t num_parameter_blocks{ 5 }; + static const std::array block_sizes = { 2, 1, 2, 1, 2 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - predict( - pose1.x(), - pose1.y(), - pose1.yaw(), - vel_linear1.x(), - vel_linear1.y(), - vel_yaw1, - acc_linear1.x(), - acc_linear1.y(), - dt, - x_pred, - y_pred, - yaw_pred, - vel_linear_x_pred, - vel_linear_y_pred, - vel_yaw2, - acc_linear_x_pred, - acc_linear_y_pred, - jacobians.data()); + predict(pose1.x(), pose1.y(), pose1.yaw(), vel_linear1.x(), vel_linear1.y(), vel_yaw1, acc_linear1.x(), + acc_linear1.y(), dt, x_pred, y_pred, yaw_pred, vel_linear_x_pred, vel_linear_y_pred, vel_yaw2, + acc_linear_x_pred, acc_linear_y_pred, jacobians.data()); jacobian << J[0], J[1], J[2], J[3], J[4]; @@ -369,43 +279,21 @@ inline void predict( * @param[in] vel_yaw2 - The second yaw velocity * @param[in] acc_linear2 - The second linear acceleration */ -inline void predict( - const tf2_2d::Transform & pose1, - const tf2_2d::Vector2 & vel_linear1, - const double vel_yaw1, - const tf2_2d::Vector2 & acc_linear1, - const double dt, - tf2_2d::Transform & pose2, - tf2_2d::Vector2 & vel_linear2, - double & vel_yaw2, - tf2_2d::Vector2 & acc_linear2) +inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, + const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, + tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2) { - double x_pred {}; - double y_pred {}; - double yaw_pred {}; - double vel_linear_x_pred {}; - double vel_linear_y_pred {}; - double acc_linear_x_pred {}; - double acc_linear_y_pred {}; - - predict( - pose1.x(), - pose1.y(), - pose1.yaw(), - vel_linear1.x(), - vel_linear1.y(), - vel_yaw1, - acc_linear1.x(), - acc_linear1.y(), - dt, - x_pred, - y_pred, - yaw_pred, - vel_linear_x_pred, - vel_linear_y_pred, - vel_yaw2, - acc_linear_x_pred, - acc_linear_y_pred); + double x_pred{}; + double y_pred{}; + double yaw_pred{}; + double vel_linear_x_pred{}; + double vel_linear_y_pred{}; + double acc_linear_x_pred{}; + double acc_linear_y_pred{}; + + predict(pose1.x(), pose1.y(), pose1.yaw(), vel_linear1.x(), vel_linear1.y(), vel_yaw1, acc_linear1.x(), + acc_linear1.y(), dt, x_pred, y_pred, yaw_pred, vel_linear_x_pred, vel_linear_y_pred, vel_yaw2, + acc_linear_x_pred, acc_linear_y_pred); pose2.setX(x_pred); pose2.setY(y_pred); diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h index 23cb9f95f..e67ff4e1b 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_cost_function.hpp instead +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_function.hpp instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp index 69f7fcbad..cdae9ce35 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -94,7 +93,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d & A); + Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -120,10 +119,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * @return The return value indicates whether the computation of the residuals and/or jacobians * was successful or not. */ - bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double position_pred_x; double position_pred_y; @@ -133,25 +129,16 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, double vel_yaw_pred; double acc_linear_pred_x; double acc_linear_pred_y; - predict( - parameters[0][0], // position1_x - parameters[0][1], // position1_y - parameters[1][0], // yaw1 - parameters[2][0], // vel_linear1_x - parameters[2][1], // vel_linear1_y - parameters[3][0], // vel_yaw1 - parameters[4][0], // acc_linear1_x - parameters[4][1], // acc_linear1_y - dt_, - position_pred_x, - position_pred_y, - yaw_pred, - vel_linear_pred_x, - vel_linear_pred_y, - vel_yaw_pred, - acc_linear_pred_x, - acc_linear_pred_y, - jacobians); + predict(parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[1][0], // yaw1 + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[3][0], // vel_yaw1 + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + dt_, position_pred_x, position_pred_y, yaw_pred, vel_linear_pred_x, vel_linear_pred_y, vel_yaw_pred, + acc_linear_pred_x, acc_linear_pred_y, jacobians); residuals[0] = parameters[5][0] - position_pred_x; residuals[1] = parameters[5][1] - position_pred_y; @@ -169,7 +156,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians) { + if (jacobians) + { // It might be possible to simplify the code below implementing something like this but using // compile-time template recursion. // @@ -185,31 +173,36 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // Update jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt yaw1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map jacobian(jacobians[1]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map jacobian(jacobians[3]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } @@ -232,31 +225,36 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // Jacobian wrt position2 - if (jacobians[5]) { + if (jacobians[5]) + { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<8, 2>(0, 0); } // Jacobian wrt yaw2 - if (jacobians[6]) { + if (jacobians[6]) + { Eigen::Map jacobian(jacobians[6]); jacobian = A_.col(2); } // Jacobian wrt vel_linear2 - if (jacobians[7]) { + if (jacobians[7]) + { Eigen::Map> jacobian(jacobians[7]); jacobian = A_.block<8, 2>(0, 3); } // Jacobian wrt vel_yaw2 - if (jacobians[8]) { + if (jacobians[8]) + { Eigen::Map jacobian(jacobians[8]); jacobian = A_.col(5); } // Jacobian wrt acc_linear2 - if (jacobians[9]) { + if (jacobians[9]) + { Eigen::Map> jacobian(jacobians[9]); jacobian = A_.block<8, 2>(0, 6); } @@ -271,11 +269,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, //!< information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction( - const double dt, - const fuse_core::Matrix8d & A) -: dt_(dt), - A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h index 68457fcc3..c7a330df2 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_functor.hpp instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp index b77458715..a37f49d69 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_models { @@ -92,7 +91,7 @@ class Unicycle2DStateCostFunctor * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d & A); + Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -108,19 +107,10 @@ class Unicycle2DStateCostFunctor * @param[in] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) * @param[out] residual - The computed residual (error) */ - template - bool operator()( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T * const position2, - const T * const yaw2, - const T * const vel_linear2, - const T * const vel_yaw2, - const T * const acc_linear2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const yaw1, const T* const vel_linear1, const T* const vel_yaw1, + const T* const acc_linear1, const T* const position2, const T* const yaw2, const T* const vel_linear2, + const T* const vel_yaw2, const T* const acc_linear2, T* residual) const; private: double dt_; @@ -128,45 +118,23 @@ class Unicycle2DStateCostFunctor //!< information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor( - const double dt, - const fuse_core::Matrix8d & A) -: dt_(dt), - A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } -template -bool Unicycle2DStateCostFunctor::operator()( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T * const position2, - const T * const yaw2, - const T * const vel_linear2, - const T * const vel_yaw2, - const T * const acc_linear2, - T * residual) const +template +bool Unicycle2DStateCostFunctor::operator()(const T* const position1, const T* const yaw1, const T* const vel_linear1, + const T* const vel_yaw1, const T* const acc_linear1, + const T* const position2, const T* const yaw2, const T* const vel_linear2, + const T* const vel_yaw2, const T* const acc_linear2, T* residual) const { T position_pred[2]; T yaw_pred[1]; T vel_linear_pred[2]; T vel_yaw_pred[1]; T acc_linear_pred[2]; - predict( - position1, - yaw1, - vel_linear1, - vel_yaw1, - acc_linear1, - T(dt_), - position_pred, - yaw_pred, - vel_linear_pred, - vel_yaw_pred, - acc_linear_pred); + predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, T(dt_), position_pred, yaw_pred, vel_linear_pred, + vel_yaw_pred, acc_linear_pred); Eigen::Map> residuals_map(residual); residuals_map(0) = position2[0] - position_pred[0]; diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h index 4500aca39..b714233a7 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_kinematic_constraint.hpp \ +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_kinematic_constraint.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp index 3f7667787..ae86596ca 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_models { @@ -93,19 +92,17 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @param[in] covariance - The covariance matrix used to weight the constraint. Order is (x, y, * yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & yaw1, - const fuse_variables::VelocityLinear2DStamped & linear_velocity1, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & yaw2, - const fuse_variables::VelocityLinear2DStamped & linear_velocity2, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration2, - const fuse_core::Matrix8d & covariance); + Unicycle2DStateKinematicConstraint(const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& yaw1, + const fuse_variables::VelocityLinear2DStamped& linear_velocity1, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, + const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& yaw2, + const fuse_variables::VelocityLinear2DStamped& linear_velocity2, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, + const fuse_core::Matrix8d& covariance); /** * @brief Destructor @@ -117,14 +114,20 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * the position1 and position2 variables in the constructor) */ - double dt() const {return dt_;} + double dt() const + { + return dt_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - const fuse_core::Matrix8d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix8d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -141,7 +144,7 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -153,10 +156,10 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - double dt_; //!< The time delta for the constraint + double dt_; //!< The time delta for the constraint fuse_core::Matrix8d sqrt_information_; //!< The square root information matrix private: @@ -170,12 +173,12 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & dt_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& dt_; + archive& sqrt_information_; } }; diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 509981c82..e720ec010 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -46,17 +46,15 @@ namespace fuse_models { Acceleration2D::Acceleration2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Acceleration2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Acceleration2D::process, this, std::placeholders::_1)) { } -void Acceleration2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Acceleration2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -74,45 +72,36 @@ void Acceleration2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " - << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); + if (params_.indices.empty()) + { + RCLCPP_WARN_STREAM(logger_, "No dimensions were specified. Data from topic " + << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Acceleration2D::onStart() { - if (!params_.indices.empty()) { + if (!params_.indices.empty()) + { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &AccelerationThrottledCallback::callback< - const geometry_msgs::msg::AccelWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&AccelerationThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -121,23 +110,14 @@ void Acceleration2D::onStop() sub_.reset(); } -void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped & msg) +void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); - common::processAccelWithCovariance( - name(), - device_id_, - msg, - params_.loss, - params_.target_frame, - params_.indices, - *tf_buffer_, - !params_.disable_checks, - *transaction, - params_.tf_timeout); + common::processAccelWithCovariance(name(), device_id_, msg, params_.loss, params_.target_frame, params_.indices, + *tf_buffer_, !params_.disable_checks, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 2692e3a27..db74fb258 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -46,16 +46,12 @@ namespace fuse_models { GraphIgnition::GraphIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1), started_(false), logger_(rclcpp::get_logger("uninitialized")) { } -void GraphIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void GraphIgnition::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -69,40 +65,26 @@ void GraphIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - fuse_core::joinTopicName(name_, params_.topic), - params_.queue_size, - std::bind(&GraphIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, fuse_core::joinTopicName(name_, params_.topic), params_.queue_size, + std::bind(&GraphIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_graph_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_graph_service), - std::bind( - &GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_graph_service), + std::bind(&GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void GraphIgnition::start() @@ -115,29 +97,32 @@ void GraphIgnition::stop() started_ = false; } -void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph & msg) +void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool GraphIgnition::setGraphServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetGraph::Request::SharedPtr req) +bool GraphIgnition::setGraphServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetGraph::Request::SharedPtr req) { - try { - process( - req->graph, - [service, request_id]() { - fuse_msgs::srv::SetGraph::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->graph, [service, request_id]() { + fuse_msgs::srv::SetGraph::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetGraph::Response response; response.success = false; response.message = e.what(); @@ -147,11 +132,11 @@ bool GraphIgnition::setGraphServiceCallback( return true; } -void GraphIgnition::process( - const fuse_msgs::msg::SerializedGraph & msg, std::function post_process) +void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg, std::function post_process) { // Verify we are in the correct state to process set graph requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the graph while the sensor is stopped."); } @@ -159,90 +144,96 @@ void GraphIgnition::process( // NOTE(methylDragon): We convert the Graph::UniquePtr to a shared pointer so it can be passed as // a copyable object to the deferred service call's std::function<> arg to // satisfy the requirement that std::function<> arguments are copyable. - const auto graph = - std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); + const auto graph = std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); // Validate the requested graph before we do anything - if (boost::empty(graph->getConstraints())) { + if (boost::empty(graph->getConstraints())) + { throw std::runtime_error("Attempting to set a graph with no constraints."); } - if (boost::empty(graph->getVariables())) { + if (boost::empty(graph->getVariables())) + { throw std::runtime_error("Attempting to set a graph with no variables."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->wait_for_service() << "' service to become available."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->wait_for_service() << "' service to become available."); } auto srv = std::make_shared(); // Don't block the executor. // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. - auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, captured_graph = std::move(graph), - msg](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the optimizer has been reset, actually send the initial state constraints to the - // optimizer - sendGraph(*captured_graph, msg.header.stamp); - if (post_process) { - post_process(); - } - }); - } else { + auto result_future = + reset_client_->async_send_request(srv, [this, post_process, captured_graph = std::move(graph), + msg](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the optimizer has been reset, actually send the initial state constraints to the + // optimizer + sendGraph(*captured_graph, msg.header.stamp); + if (post_process) + { + post_process(); + } + }); + } + else + { sendGraph(*graph, msg.header.stamp); - if (post_process) { + if (post_process) + { post_process(); } } } -void GraphIgnition::sendGraph(const fuse_core::Graph & graph, const rclcpp::Time & stamp) +void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp) { // Create a transaction equivalent to the graph auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(stamp); // Add variables - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { transaction->addVariable(variable.clone()); // If the variable is a fuse_variables::Stamped variable, set the involved stamp - const auto stamped_variable = dynamic_cast(&variable); - if (stamped_variable) { + const auto stamped_variable = dynamic_cast(&variable); + if (stamped_variable) + { transaction->addInvolvedStamp(stamped_variable->stamp()); } } // If the transaction ended up with no involved stamps, we use a single involved stamped equal to // the transaction/graph stamp - if (boost::empty(transaction->involvedStamps())) { + if (boost::empty(transaction->involvedStamps())) + { transaction->addInvolvedStamp(stamp); } // Add constraints - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { transaction->addConstraint(constraint.clone()); } // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_graph request (stamp: " - << transaction->stamp().nanoseconds() << ", constraints: " - << boost::size(transaction->addedConstraints()) << ", variables: " - << boost::size(transaction->addedVariables()) << ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_graph request (stamp: " + << transaction->stamp().nanoseconds() + << ", constraints: " << boost::size(transaction->addedConstraints()) + << ", variables: " << boost::size(transaction->addedVariables()) << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 1a619f660..2521f5dfb 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -45,7 +45,6 @@ #include #include - // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::Imu2D, fuse_core::SensorModel) @@ -53,17 +52,15 @@ namespace fuse_models { Imu2D::Imu2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Imu2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Imu2D::process, this, std::placeholders::_1)) { } -void Imu2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Imu2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -81,35 +78,29 @@ void Imu2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.orientation_indices.empty() && - params_.linear_acceleration_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Imu2D::onStart() { - if (!params_.orientation_indices.empty() || - !params_.linear_acceleration_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.orientation_indices.empty() || !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,16 +108,10 @@ void Imu2D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &ImuThrottledCallback::callback, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -135,7 +120,7 @@ void Imu2D::onStop() sub_.reset(); } -void Imu2D::process(const sensor_msgs::msg::Imu & msg) +void Imu2D::process(const sensor_msgs::msg::Imu& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -170,37 +155,21 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.orientation_target_frame, - {}, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, *pose, params_.pose_loss, + params_.orientation_target_frame, {}, params_.orientation_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); } // Handle the twist data (only include indices for angular velocity) - common::processTwistWithCovariance( - name(), - device_id_, - twist, - nullptr, - params_.angular_velocity_loss, - params_.twist_target_frame, - {}, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, twist, nullptr, params_.angular_velocity_loss, + params_.twist_target_frame, {}, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Handle the acceleration data geometry_msgs::msg::AccelWithCovarianceStamped accel; @@ -217,7 +186,8 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity - if (params_.remove_gravitational_acceleration) { + if (params_.remove_gravitational_acceleration) + { geometry_msgs::msg::Vector3 accel_gravity; accel_gravity.z = params_.gravitational_acceleration; geometry_msgs::msg::TransformStamped orientation_trans; @@ -230,87 +200,64 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) accel.accel.accel.linear.z -= accel_gravity.z; } - common::processAccelWithCovariance( - name(), - device_id_, - accel, - params_.linear_acceleration_loss, - params_.acceleration_target_frame, - params_.linear_acceleration_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processAccelWithCovariance(name(), device_id_, accel, params_.linear_acceleration_loss, + params_.acceleration_target_frame, params_.linear_acceleration_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Imu2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction) +void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. - orientation_target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() - << " to orientation target frame " << - params_.orientation_target_frame); + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to orientation target frame " + << params_.orientation_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPoseWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); } - } else { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + else + { + common::processDifferentialPoseWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, {}, + params_.orientation_indices, validate, transaction); + } + } + else + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, {}, params_.orientation_indices, validate, + transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index 192d56b89..e9f5d43b5 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -51,17 +51,15 @@ namespace fuse_models { Odometry2D::Odometry2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Odometry2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Odometry2D::process, this, std::placeholders::_1)) { } -void Odometry2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Odometry2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -79,37 +77,29 @@ void Odometry2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.position_indices.empty() && - params_.orientation_indices.empty() && - params_.linear_velocity_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.position_indices.empty() && params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Odometry2D::onStart() { - if (!params_.position_indices.empty() || - !params_.orientation_indices.empty() || - !params_.linear_velocity_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,17 +107,10 @@ void Odometry2D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &OdometryThrottledCallback::callback< - const nav_msgs::msg::Odometry &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -136,7 +119,7 @@ void Odometry2D::onStop() sub_.reset(); } -void Odometry2D::process(const nav_msgs::msg::Odometry & msg) +void Odometry2D::process(const nav_msgs::msg::Odometry& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -154,105 +137,78 @@ void Odometry2D::process(const nav_msgs::msg::Odometry & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.pose_target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); } // Handle the twist data - common::processTwistWithCovariance( - name(), - device_id_, - twist, - params_.linear_velocity_loss, - params_.angular_velocity_loss, - params_.twist_target_frame, - params_.linear_velocity_indices, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, twist, params_.linear_velocity_loss, + params_.angular_velocity_loss, params_.twist_target_frame, + params_.linear_velocity_indices, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Odometry2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction) +void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " - << rclcpp::Time( - pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to pose target frame " + << params_.pose_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPoseWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - transformed_twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPoseWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + transformed_twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, + params_.position_indices, params_.orientation_indices, + validate, transaction); } - } else { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + } + else + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, params_.position_indices, + params_.orientation_indices, validate, transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index a4f108e4a..e8a67f04e 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -61,17 +61,16 @@ namespace fuse_models { Odometry2DPublisher::Odometry2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + , latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { } void Odometry2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -89,54 +88,45 @@ void Odometry2DPublisher::onInit() params_.loadFromROS(interfaces_, name_); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { tf_buffer_ = std::make_unique( - clock_, - params_.tf_cache_time.to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ + clock_, params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ ); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface()); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } // Advertise the topics rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - odom_pub_ = rclcpp::create_publisher( - interfaces_, - params_.topic, - params_.queue_size, - pub_options); + odom_pub_ = + rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); acceleration_pub_ = rclcpp::create_publisher( - interfaces_, - params_.acceleration_topic, - params_.queue_size, - pub_options); + interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); } -void Odometry2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Odometry2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { + if (0u == latest_stamp.nanoseconds()) + { { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; } - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of state variables with device id '" - << device_id_ << "'."); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" << device_id_ + << "'."); return; } @@ -149,17 +139,8 @@ void Odometry2DPublisher::notifyCallback( nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; - if (!getState( - *graph, - latest_stamp, - device_id_, - position_uuid, - orientation_uuid, - velocity_linear_uuid, - velocity_angular_uuid, - acceleration_linear_uuid, - odom_output, - acceleration_output)) + if (!getState(*graph, latest_stamp, device_id_, position_uuid, orientation_uuid, velocity_linear_uuid, + velocity_angular_uuid, acceleration_linear_uuid, odom_output, acceleration_output)) { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; @@ -176,14 +157,16 @@ void Odometry2DPublisher::notifyCallback( // Don't waste CPU computing the covariance if nobody is listening rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; bool latest_covariance_valid = latest_covariance_valid_; - if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) + { // Throttle covariance computation if (params_.covariance_throttle_period.nanoseconds() == 0 || - latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) { latest_covariance_stamp = latest_stamp; - try { + try + { std::vector> covariance_requests; covariance_requests.emplace_back(position_uuid, position_uuid); covariance_requests.emplace_back(position_uuid, orientation_uuid); @@ -222,22 +205,21 @@ void Odometry2DPublisher::notifyCallback( acceleration_output.accel.covariance[7] = covariance_matrices[6][3]; latest_covariance_valid = true; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - logger_, - "An error occurred computing the covariance information for " - << latest_stamp.nanoseconds() - << ". The covariance will be set to zero.\n" - << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" + << e.what()); std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); - std::fill( - acceleration_output.accel.covariance.begin(), - acceleration_output.accel.covariance.end(), 0.0); + std::fill(acceleration_output.accel.covariance.begin(), acceleration_output.accel.covariance.end(), 0.0); latest_covariance_valid = false; } - } else { + } + else + { // This covariance computation cycle has been skipped, so simply take the last covariance // computed // @@ -269,13 +251,9 @@ void Odometry2DPublisher::onStart() acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); // TODO(CH3): Add this to a separate callback group for async behavior - publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / params_.publish_frequency), - std::move(std::bind(&Odometry2DPublisher::publishTimerCallback, this)), - cb_group_ - ); + publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry2DPublisher::publishTimerCallback, this)), cb_group_); delayed_throttle_filter_.reset(); } @@ -285,39 +263,33 @@ void Odometry2DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry2DPublisher::getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +bool Odometry2DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, + fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, + fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, + nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration) { - try { + try + { position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear2DStamped(stamp, device_id).uuid(); - auto velocity_linear_variable = dynamic_cast( - graph.getVariable(velocity_linear_uuid)); + auto velocity_linear_variable = + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular2DStamped(stamp, device_id).uuid(); - auto velocity_angular_variable = dynamic_cast( - graph.getVariable(velocity_angular_uuid)); + auto velocity_angular_variable = + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear2DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast( - graph.getVariable(acceleration_linear_uuid)); + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); @@ -336,15 +308,17 @@ bool Odometry2DPublisher::getState( acceleration.accel.accel.angular.x = 0.0; acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } @@ -368,10 +342,10 @@ void Odometry2DPublisher::publishTimerCallback() acceleration_output = acceleration_output_; } - if (0u == latest_stamp.nanoseconds()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, delayed_throttle_filter_.isEnabled(), - "No valid state data yet. Delaying tf broadcast."); + if (0u == latest_stamp.nanoseconds()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); return; } @@ -379,7 +353,8 @@ void Odometry2DPublisher::publishTimerCallback() tf2::fromMsg(odom_output.pose.pose, pose); // If requested, we need to project our state forward in time using the 2D kinematic model - if (params_.predict_to_current_time) { + if (params_.predict_to_current_time) + { rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); tf2_2d::Vector2 velocity_linear; tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); @@ -389,23 +364,15 @@ void Odometry2DPublisher::publishTimerCallback() fuse_core::Matrix8d jacobian; tf2_2d::Vector2 acceleration_linear; - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { tf2::fromMsg(acceleration_output.accel.accel.linear, acceleration_linear); } double yaw_vel; - predict( - pose, - velocity_linear, - odom_output.twist.twist.angular.z, - acceleration_linear, - dt, - pose, - velocity_linear, - yaw_vel, - acceleration_linear, - jacobian); + predict(pose, velocity_linear, odom_output.twist.twist.angular.z, acceleration_linear, dt, pose, velocity_linear, + yaw_vel, acceleration_linear, jacobian); odom_output.pose.pose.position.x = pose.getX(); odom_output.pose.pose.position.y = pose.getY(); @@ -415,7 +382,8 @@ void Odometry2DPublisher::publishTimerCallback() odom_output.twist.twist.linear.y = velocity_linear.y(); odom_output.twist.twist.angular.z = yaw_vel; - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { acceleration_output.accel.accel.linear.x = acceleration_linear.x(); acceleration_output.accel.accel.linear.y = acceleration_linear.y(); } @@ -425,7 +393,8 @@ void Odometry2DPublisher::publishTimerCallback() // Either the last covariance computation was skipped because there was no subscriber, // or it failed - if (latest_covariance_valid) { + if (latest_covariance_valid) + { fuse_core::Matrix8d covariance; covariance(0, 0) = odom_output.pose.covariance[0]; covariance(0, 1) = odom_output.pose.covariance[1]; @@ -463,10 +432,10 @@ void Odometry2DPublisher::publishTimerCallback() covariance = jacobian * covariance * jacobian.transpose(); auto process_noise_covariance = params_.process_noise_covariance; - if (params_.scale_process_noise) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, velocity_linear, - odom_output.twist.twist.angular.z, params_.velocity_norm_min); + if (params_.scale_process_noise) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, + odom_output.twist.twist.angular.z, params_.velocity_norm_min); } covariance.noalias() += dt * process_noise_covariance; @@ -501,11 +470,13 @@ void Odometry2DPublisher::publishTimerCallback() odom_pub_->publish(odom_output); acceleration_pub_->publish(acceleration_output); - if (params_.publish_tf) { + if (params_.publish_tf) + { auto frame_id = odom_output.header.frame_id; auto child_frame_id = odom_output.child_frame_id; - if (params_.invert_tf) { + if (params_.invert_tf) + { pose = pose.inverse(); std::swap(frame_id, child_frame_id); } @@ -519,23 +490,24 @@ void Odometry2DPublisher::publishTimerCallback() trans.transform.translation.z = odom_output.pose.pose.position.z; trans.transform.rotation = tf2::toMsg(pose.getRotation()); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - try { - auto base_to_odom = tf_buffer_->lookupTransform( - params_.base_link_frame_id, - params_.odom_frame_id, - trans.header.stamp, - params_.tf_timeout); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, trans); map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" - << params_.odom_frame_id << " transform. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id + << " transform. Error: " << e.what()); return; } diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 0066ee7b5..53ae1ce51 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -49,17 +49,15 @@ namespace fuse_models { Pose2D::Pose2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Pose2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Pose2D::process, this, std::placeholders::_1)) { } -void Pose2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Pose2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -77,49 +75,36 @@ void Pose2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.position_indices.empty() && - params_.orientation_indices.empty()) + if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Pose2D::onStart() { - if (!params_.position_indices.empty() || - !params_.orientation_indices.empty()) + if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &PoseThrottledCallback::callback< - const geometry_msgs::msg::PoseWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&PoseThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -128,7 +113,7 @@ void Pose2D::onStop() sub_.reset(); } -void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -136,56 +121,42 @@ void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(msg, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - msg, - params_.loss, - params_.target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, msg, params_.loss, params_.target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); } // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Pose2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const bool validate, - fuse_core::Transaction & transaction) +void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); - transformed_pose->header.frame_id = - params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() - << " to target frame " << params_.target_frame); + transformed_pose->header.frame_id = params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to target frame " + << params_.target_frame); return; } - if (previous_pose_msg_) { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_msg_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + if (previous_pose_msg_) + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_msg_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.loss, params_.position_indices, params_.orientation_indices, + validate, transaction); } previous_pose_msg_ = std::move(transformed_pose); diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index 635c6baca..dc3392315 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -42,15 +42,12 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Transaction, fuse_core::SensorModel) namespace fuse_models { -Transaction::Transaction() -: fuse_core::AsyncSensorModel(1) +Transaction::Transaction() : fuse_core::AsyncSensorModel(1) { } -void Transaction::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Transaction::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -68,12 +65,8 @@ void Transaction::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - fuse_core::joinTopicName(name_, params_.topic), - params_.queue_size, - std::bind(&Transaction::process, this, std::placeholders::_1), - sub_options - ); + interfaces_, fuse_core::joinTopicName(name_, params_.topic), params_.queue_size, + std::bind(&Transaction::process, this, std::placeholders::_1), sub_options); } void Transaction::onStop() @@ -81,7 +74,7 @@ void Transaction::onStop() sub_.reset(); } -void Transaction::process(const fuse_msgs::msg::SerializedTransaction & msg) +void Transaction::process(const fuse_msgs::msg::SerializedTransaction& msg) { // Deserialize and send the transaction to the plugin's parent sendTransaction(transaction_deserializer_.deserialize(msg)->clone()); diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index b244971e5..83bf129de 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -46,17 +46,15 @@ namespace fuse_models { Twist2D::Twist2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Twist2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Twist2D::process, this, std::placeholders::_1)) { } -void Twist2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Twist2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -74,49 +72,36 @@ void Twist2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.linear_indices.empty() && - params_.angular_indices.empty()) + if (params_.linear_indices.empty() && params_.angular_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Twist2D::onStart() { - if (!params_.linear_indices.empty() || - !params_.angular_indices.empty()) + if (!params_.linear_indices.empty() || !params_.angular_indices.empty()) { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &TwistThrottledCallback::callback< - const geometry_msgs::msg::TwistWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&TwistThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -125,25 +110,15 @@ void Twist2D::onStop() sub_.reset(); } -void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped & msg) +void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); - common::processTwistWithCovariance( - name(), - device_id_, - msg, - params_.linear_loss, - params_.angular_loss, - params_.target_frame, - params_.linear_indices, - params_.angular_indices, - *tf_buffer_, - !params_.disable_checks, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, msg, params_.linear_loss, params_.angular_loss, + params_.target_frame, params_.linear_indices, params_.angular_indices, *tf_buffer_, + !params_.disable_checks, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 00f71e18a..9930b921a 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -65,25 +65,24 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle2D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const tf2_2d::Vector2 & vector) +inline bool isfinite(const tf2_2d::Vector2& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y()); } -inline bool isfinite(const tf2_2d::Transform & transform) +inline bool isfinite(const tf2_2d::Transform& transform) { - return std::isfinite(transform.x()) && std::isfinite(transform.y()) && std::isfinite( - transform.yaw()); + return std::isfinite(transform.x()) && std::isfinite(transform.y()) && std::isfinite(transform.yaw()); } -std::string to_string(const tf2_2d::Vector2 & vector) +std::string to_string(const tf2_2d::Vector2& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const tf2_2d::Transform & transform) +std::string to_string(const tf2_2d::Transform& transform) { std::ostringstream oss; oss << transform; @@ -95,21 +94,20 @@ std::string to_string(const tf2_2d::Transform & transform) namespace fuse_core { -template -inline void validateCovariance( - const Eigen::DenseBase & covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +template +inline void validateCovariance(const Eigen::DenseBase& covariance, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -119,24 +117,25 @@ namespace fuse_models { Unicycle2D::Unicycle2D() -: fuse_core::AsyncMotionModel(1), - logger_(rclcpp::get_logger("uninitialized")), - buffer_length_(rclcpp::Duration::max()), - device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Unicycle2D::generateMotionModel, this, rclcpp::Duration::max()) + : fuse_core::AsyncMotionModel(1) + , logger_(rclcpp::get_logger("uninitialized")) + , buffer_length_(rclcpp::Duration::max()) + , device_id_(fuse_core::uuid::NIL) + , timestamp_manager_(&Unicycle2D::generateMotionModel, this, rclcpp::Duration::max()) { } -void Unicycle2D::print(std::ostream & stream) const +void Unicycle2D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto & state : state_history_) { + for (const auto& state : state_history_) + { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); } } -void Unicycle2D::StateHistoryElement::print(std::ostream & stream) const +void Unicycle2D::StateHistoryElement::print(std::ostream& stream) const { stream << " position uuid: " << position_uuid << "\n" << " yaw uuid: " << yaw_uuid << "\n" @@ -151,34 +150,40 @@ void Unicycle2D::StateHistoryElement::print(std::ostream & stream) const void Unicycle2D::StateHistoryElement::validate() const { - if (!std::isfinite(pose)) { + if (!std::isfinite(pose)) + { throw std::runtime_error("Invalid pose " + std::to_string(pose)); } - if (!std::isfinite(velocity_linear)) { + if (!std::isfinite(velocity_linear)) + { throw std::runtime_error("Invalid linear velocity " + std::to_string(velocity_linear)); } - if (!std::isfinite(velocity_yaw)) { + if (!std::isfinite(velocity_yaw)) + { throw std::runtime_error("Invalid yaw velocity " + std::to_string(velocity_yaw)); } - if (!std::isfinite(acceleration_linear)) { + if (!std::isfinite(acceleration_linear)) + { throw std::runtime_error("Invalid linear acceleration " + std::to_string(acceleration_linear)); } } -bool Unicycle2D::applyCallback(fuse_core::Transaction & transaction) +bool Unicycle2D::applyCallback(fuse_core::Transaction& transaction) { // Use the timestamp manager to generate just the required motion model segments. The timestamp // manager, in turn, makes calls to the generateMotionModel() function. - try { + try + { // Now actually generate the motion model segments timestamp_manager_.query(transaction, true); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "An error occurred while completing the motion model query. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); return false; } return true; @@ -189,9 +194,8 @@ void Unicycle2D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } -void Unicycle2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Unicycle2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -203,55 +207,33 @@ void Unicycle2D::onInit() clock_ = interfaces_.get_node_clock_interface()->get_clock(); std::vector process_noise_diagonal; - process_noise_diagonal = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "process_noise_diagonal"), - process_noise_diagonal); - - if (process_noise_diagonal.size() != 8) { + process_noise_diagonal = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "process_noise_diagonal"), process_noise_diagonal); + + if (process_noise_diagonal.size() != 8) + { throw std::runtime_error("Process noise diagonal must be of length 8!"); } process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); - scale_process_noise_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "scale_process_noise"), - scale_process_noise_); + scale_process_noise_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "scale_process_noise"), + scale_process_noise_); velocity_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_norm_min"), - velocity_norm_min_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "velocity_norm_min"), velocity_norm_min_); disable_checks_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "disable_checks"), - disable_checks_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "disable_checks"), disable_checks_); double buffer_length = 3.0; - buffer_length = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "buffer_length"), - buffer_length); - - if (buffer_length < 0.0) { - throw std::runtime_error( - "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + buffer_length = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "buffer_length"), buffer_length); + + if (buffer_length < 0.0) + { + throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } - buffer_length_ = - (buffer_length == - 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + buffer_length_ = (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); timestamp_manager_.bufferLength(buffer_length_); device_id_ = fuse_variables::loadDeviceId(interfaces_); @@ -263,29 +245,27 @@ void Unicycle2D::onStart() state_history_.clear(); } -void Unicycle2D::generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) +void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) { - assert( - beginning_stamp < ending_stamp || - (beginning_stamp == ending_stamp && state_history_.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); StateHistoryElement base_state; - rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + rclcpp::Time base_time{ 0, 0, RCL_ROS_TIME }; // Find an entry that is > beginning_stamp // The entry that is <= will be the one before it auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); - if (base_state_pair_it == state_history_.begin()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, !state_history_.empty(), - "Unable to locate a state in this history with stamp <= " - << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + if (base_state_pair_it == state_history_.begin()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); base_time = beginning_stamp; - } else { + } + else + { --base_state_pair_it; base_time = base_state_pair_it->first; base_state = base_state_pair_it->second; @@ -295,33 +275,27 @@ void Unicycle2D::generateMotionModel( // If the nearest state we had was before the beginning stamp, we need to project that state to // the beginning stamp - if (base_time != beginning_stamp) { - predict( - base_state.pose, - base_state.velocity_linear, - base_state.velocity_yaw, - base_state.acceleration_linear, - (beginning_stamp - base_time).seconds(), - state1.pose, - state1.velocity_linear, - state1.velocity_yaw, - state1.acceleration_linear); - } else { + if (base_time != beginning_stamp) + { + predict(base_state.pose, base_state.velocity_linear, base_state.velocity_yaw, base_state.acceleration_linear, + (beginning_stamp - base_time).seconds(), state1.pose, state1.velocity_linear, state1.velocity_yaw, + state1.acceleration_linear); + } + else + { state1 = base_state; } // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); - if (dt == 0.0) { + if (dt == 0.0) + { state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid(); state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_linear_uuid = - fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_yaw_uuid = - fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); - state1.acc_linear_uuid = - fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); state_history_.emplace(beginning_stamp, std::move(state1)); @@ -330,39 +304,20 @@ void Unicycle2D::generateMotionModel( // Now predict to get an initial guess for the state at the ending stamp StateHistoryElement state2; - predict( - state1.pose, - state1.velocity_linear, - state1.velocity_yaw, - state1.acceleration_linear, - dt, - state2.pose, - state2.velocity_linear, - state2.velocity_yaw, - state2.acceleration_linear); + predict(state1.pose, state1.velocity_linear, state1.velocity_yaw, state1.acceleration_linear, dt, state2.pose, + state2.velocity_linear, state2.velocity_yaw, state2.acceleration_linear); // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared( - beginning_stamp, - device_id_); - auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared( - beginning_stamp, - device_id_); - auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared( - beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_); auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_); auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_); - auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared( - ending_stamp, - device_id_); - auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared( - ending_stamp, - device_id_); - auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared( - ending_stamp, - device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_); + auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_); position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x(); position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y(); @@ -370,20 +325,16 @@ void Unicycle2D::generateMotionModel( velocity_linear1->data()[fuse_variables::VelocityLinear2DStamped::X] = state1.velocity_linear.x(); velocity_linear1->data()[fuse_variables::VelocityLinear2DStamped::Y] = state1.velocity_linear.y(); velocity_yaw1->data()[fuse_variables::VelocityAngular2DStamped::YAW] = state1.velocity_yaw; - acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::X] = - state1.acceleration_linear.x(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::Y] = - state1.acceleration_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::X] = state1.acceleration_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::Y] = state1.acceleration_linear.y(); position2->data()[fuse_variables::Position2DStamped::X] = state2.pose.x(); position2->data()[fuse_variables::Position2DStamped::Y] = state2.pose.y(); yaw2->data()[fuse_variables::Orientation2DStamped::YAW] = state2.pose.yaw(); velocity_linear2->data()[fuse_variables::VelocityLinear2DStamped::X] = state2.velocity_linear.x(); velocity_linear2->data()[fuse_variables::VelocityLinear2DStamped::Y] = state2.velocity_linear.y(); velocity_yaw2->data()[fuse_variables::VelocityAngular2DStamped::YAW] = state2.velocity_yaw; - acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::X] = - state2.acceleration_linear.x(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::Y] = - state2.acceleration_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::X] = state2.acceleration_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::Y] = state2.acceleration_linear.y(); state1.position_uuid = position1->uuid(); state1.yaw_uuid = yaw1->uuid(); @@ -401,40 +352,33 @@ void Unicycle2D::generateMotionModel( // Scale process noise covariance pose by the norm of the current state twist auto process_noise_covariance = process_noise_covariance_; - if (scale_process_noise_) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, - velocity_norm_min_); + if (scale_process_noise_) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, + velocity_norm_min_); } // Validate process_noise_covariance *= dt; - if (!disable_checks_) { - try { + if (!disable_checks_) + { + try + { validateMotionModel(state1, state2, process_noise_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Invalid '" << name_ << "' motion model: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); return; } } // Create the constraints for this motion model segment auto constraint = fuse_models::Unicycle2DStateKinematicConstraint::make_shared( - name(), - *position1, - *yaw1, - *velocity_linear1, - *velocity_yaw1, - *acceleration_linear1, - *position2, - *yaw2, - *velocity_linear2, - *velocity_yaw2, - *acceleration_linear2, - process_noise_covariance); + name(), *position1, *yaw1, *velocity_linear1, *velocity_yaw1, *acceleration_linear1, *position2, *yaw2, + *velocity_linear2, *velocity_yaw2, *acceleration_linear2, process_noise_covariance); // Update the output variables constraints.push_back(constraint); @@ -450,22 +394,24 @@ void Unicycle2D::generateMotionModel( variables.push_back(acceleration_linear2); } -void Unicycle2D::updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length) +void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length) { - if (state_history.empty()) { + if (state_history.empty()) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = state_history.rbegin()->first; + const auto& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length.seconds()) { + if (ending_stamp.seconds() > buffer_length.seconds()) + { expiration_time = ending_stamp - buffer_length; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -475,7 +421,8 @@ void Unicycle2D::updateStateHistoryEstimates( // - at least one entry remains at all times // - the history covers *at least* until the expiration time. Longer is acceptable. auto expiration_iter = state_history.upper_bound(expiration_time); - if (expiration_iter != state_history.begin()) { + if (expiration_iter != state_history.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time state_history.erase(state_history.begin(), std::prev(expiration_iter)); @@ -484,87 +431,79 @@ void Unicycle2D::updateStateHistoryEstimates( // Update the states in the state history with information from the graph // If a state is not in the graph yet, predict the state in question from the closest previous // state - for (auto current_iter = state_history.begin(); current_iter != state_history.end(); - ++current_iter) + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto & current_stamp = current_iter->first; - auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && - graph.variableExists(current_state.yaw_uuid) && - graph.variableExists(current_state.vel_linear_uuid) && - graph.variableExists(current_state.vel_yaw_uuid) && - graph.variableExists(current_state.acc_linear_uuid)) + const auto& current_stamp = current_iter->first; + auto& current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.yaw_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_yaw_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto & position = graph.getVariable(current_state.position_uuid); - const auto & yaw = graph.getVariable(current_state.yaw_uuid); - const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto & vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); - const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + const auto& position = graph.getVariable(current_state.position_uuid); + const auto& yaw = graph.getVariable(current_state.yaw_uuid); + const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto& vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); + const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.pose.setX(position.data()[fuse_variables::Position2DStamped::X]); current_state.pose.setY(position.data()[fuse_variables::Position2DStamped::Y]); current_state.pose.setAngle(yaw.data()[fuse_variables::Orientation2DStamped::YAW]); - current_state.velocity_linear.setX( - vel_linear.data()[fuse_variables::VelocityLinear2DStamped:: - X]); - current_state.velocity_linear.setY( - vel_linear.data()[fuse_variables::VelocityLinear2DStamped:: - Y]); + current_state.velocity_linear.setX(vel_linear.data()[fuse_variables::VelocityLinear2DStamped::X]); + current_state.velocity_linear.setY(vel_linear.data()[fuse_variables::VelocityLinear2DStamped::Y]); current_state.velocity_yaw = vel_yaw.data()[fuse_variables::VelocityAngular2DStamped::YAW]; - current_state.acceleration_linear.setX( - acc_linear.data()[fuse_variables:: - AccelerationLinear2DStamped::X]); - current_state.acceleration_linear.setY( - acc_linear.data()[fuse_variables:: - AccelerationLinear2DStamped::Y]); - } else if (current_iter != state_history.begin()) { + current_state.acceleration_linear.setX(acc_linear.data()[fuse_variables::AccelerationLinear2DStamped::X]); + current_state.acceleration_linear.setY(acc_linear.data()[fuse_variables::AccelerationLinear2DStamped::Y]); + } + else if (current_iter != state_history.begin()) + { auto previous_iter = std::prev(current_iter); - const auto & previous_stamp = previous_iter->first; - const auto & previous_state = previous_iter->second; + const auto& previous_stamp = previous_iter->first; + const auto& previous_state = previous_iter->second; // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction // logic, to provide a more accurate update to this state. - predict( - previous_state.pose, - previous_state.velocity_linear, - previous_state.velocity_yaw, - previous_state.acceleration_linear, - (current_stamp - previous_stamp).seconds(), - current_state.pose, - current_state.velocity_linear, - current_state.velocity_yaw, - current_state.acceleration_linear); + predict(previous_state.pose, previous_state.velocity_linear, previous_state.velocity_yaw, + previous_state.acceleration_linear, (current_stamp - previous_stamp).seconds(), current_state.pose, + current_state.velocity_linear, current_state.velocity_yaw, current_state.acceleration_linear); } } } -void Unicycle2D::validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix8d & process_noise_covariance) +void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix8d& process_noise_covariance) { - try { + try + { state1.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } - try { + try + { state2.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } - try { + try + { fuse_core::validateCovariance(process_noise_covariance); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream & operator<<(std::ostream & stream, const Unicycle2D & unicycle_2d) +std::ostream& operator<<(std::ostream& stream, const Unicycle2D& unicycle_2d) { unicycle_2d.print(stream); return stream; diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 774eb4c3e..1bec4e0fa 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -66,18 +66,16 @@ namespace fuse_models { Unicycle2DIgnition::Unicycle2DIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - initial_transaction_sent_(false), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1) + , started_(false) + , initial_transaction_sent_(false) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) { } -void Unicycle2DIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Unicycle2DIgnition::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -94,52 +92,32 @@ void Unicycle2DIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_pose_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_service), - std::bind( - &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), + std::bind(&Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); set_pose_deprecated_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_deprecated_service), - std::bind( - &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), + std::bind(&Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void Unicycle2DIgnition::start() @@ -149,13 +127,13 @@ void Unicycle2DIgnition::start() // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once // ever? I feel like it should be "only once ever". Send an initial state // transaction immediately, if requested - if (params_.publish_on_startup && !initial_transaction_sent_) { + if (params_.publish_on_startup && !initial_transaction_sent_) + { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; - pose.pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); + pose.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); pose.pose.covariance[0] = params_.initial_sigma[0] * params_.initial_sigma[0]; pose.pose.covariance[7] = params_.initial_sigma[1] * params_.initial_sigma[1]; pose.pose.covariance[35] = params_.initial_sigma[2] * params_.initial_sigma[2]; @@ -169,30 +147,32 @@ void Unicycle2DIgnition::stop() started_ = false; } -void Unicycle2DIgnition::subscriberCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Unicycle2DIgnition::setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPose::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPose::Response response; response.success = false; response.message = e.what(); @@ -203,18 +183,18 @@ bool Unicycle2DIgnition::setPoseServiceCallback( } bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr service, std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPoseDeprecated::Response response; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); service->send_response(*request_id, response); @@ -222,63 +202,62 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Unicycle2DIgnition::process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + std::function post_process) { // Verify we are in the correct state to process set pose requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { - throw std::invalid_argument( - "Attempting to set the pose to an invalid position (" + - std::to_string(pose.pose.pose.position.x) + ", " + - std::to_string(pose.pose.pose.position.y) + ")."); + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) + { + throw std::invalid_argument("Attempting to set the pose to an invalid position (" + + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ")."); } - auto orientation_norm = std::sqrt( - pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); - if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid orientation (" + - std::to_string(pose.pose.pose.orientation.x) + ", " + - std::to_string(pose.pose.pose.orientation.y) + ", " + - std::to_string(pose.pose.pose.orientation.z) + ", " + - std::to_string(pose.pose.pose.orientation.w) + ")."); + "Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); } auto position_cov = fuse_core::Matrix2d(); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; - if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric position covariance matri\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[6], pose.pose.covariance[7]; + if (!fuse_core::isSymmetric(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matri\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + if (!fuse_core::isPositiveDefinite(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } auto orientation_cov = fuse_core::Matrix1d(); orientation_cov << pose.pose.covariance[35]; - if (orientation_cov(0) <= 0.0) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite orientation covariance " - "matrix " + fuse_core::to_string(orientation_cov) + "."); + if (orientation_cov(0) <= 0.0) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite orientation covariance " + "matrix " + + fuse_core::to_string(orientation_cov) + "."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->get_service_name() << "' service to become available."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become available."); } auto srv = std::make_shared(); @@ -286,27 +265,30 @@ void Unicycle2DIgnition::process( // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, pose](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the pose has been validated and the optimizer has been reset, actually send the - // initial state constraints to the optimizer - sendPrior(pose); - if (post_process) { - post_process(); - } - }); - } else { + srv, [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) + { + post_process(); + } + }); + } + else + { sendPrior(pose); - if (post_process) { + if (post_process) + { post_process(); } } } -void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { - const auto & stamp = pose.header.stamp; + const auto& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped @@ -315,19 +297,14 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS position->x() = pose.pose.pose.position.x; position->y() = pose.pose.pose.position.y; auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); - orientation->yaw() = fuse_core::getYaw( - pose.pose.pose.orientation.w, - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z); + orientation->yaw() = fuse_core::getYaw(pose.pose.pose.orientation.w, pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, pose.pose.pose.orientation.z); auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); linear_velocity->x() = params_.initial_state[3]; linear_velocity->y() = params_.initial_state[4]; auto angular_velocity = fuse_variables::VelocityAngular2DStamped::make_shared(stamp, device_id_); angular_velocity->yaw() = params_.initial_state[5]; - auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared( - stamp, - device_id_); + auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared(stamp, device_id_); linear_acceleration->x() = params_.initial_state[6]; linear_acceleration->y() = params_.initial_state[7]; @@ -337,8 +314,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto position_mean = fuse_core::VectorXd(2); position_mean << position->x(), position->y(); auto position_cov = fuse_core::MatrixXd(2, 2); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[6], pose.pose.covariance[7]; auto orientation_mean = fuse_core::VectorXd(1); orientation_mean << orientation->yaw(); auto orientation_cov = fuse_core::MatrixXd(1, 1); @@ -346,8 +322,8 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto linear_velocity_mean = fuse_core::VectorXd(2); linear_velocity_mean << linear_velocity->x(), linear_velocity->y(); auto linear_velocity_cov = fuse_core::MatrixXd(2, 2); - linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, - 0.0, params_.initial_sigma[4] * params_.initial_sigma[4]; + linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, 0.0, + params_.initial_sigma[4] * params_.initial_sigma[4]; auto angular_velocity_mean = fuse_core::VectorXd(1); angular_velocity_mean << angular_velocity->yaw(); auto angular_velocity_cov = fuse_core::MatrixXd(1, 1); @@ -355,39 +331,20 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto linear_acceleration_mean = fuse_core::VectorXd(2); linear_acceleration_mean << linear_acceleration->x(), linear_acceleration->y(); auto linear_acceleration_cov = fuse_core::MatrixXd(2, 2); - linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7]; + linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, + params_.initial_sigma[7] * params_.initial_sigma[7]; // Create absolute constraints for each variable auto position_constraint = fuse_constraints::AbsolutePosition2DStampedConstraint::make_shared( - name(), - *position, - position_mean, - position_cov); - auto orientation_constraint = - fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( - name(), - *orientation, - orientation_mean, - orientation_cov); - auto linear_velocity_constraint = - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( - name(), - *linear_velocity, - linear_velocity_mean, - linear_velocity_cov); - auto angular_velocity_constraint = - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( - name(), - *angular_velocity, - angular_velocity_mean, - angular_velocity_cov); - auto linear_acceleration_constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - name(), - *linear_acceleration, - linear_acceleration_mean, - linear_acceleration_cov); + name(), *position, position_mean, position_cov); + auto orientation_constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + name(), *orientation, orientation_mean, orientation_cov); + auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + name(), *linear_velocity, linear_velocity_mean, linear_velocity_cov); + auto angular_velocity_constraint = fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + name(), *angular_velocity, angular_velocity_mean, angular_velocity_cov); + auto linear_acceleration_constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + name(), *linear_acceleration, linear_acceleration_mean, linear_acceleration_cov); // Create the transaction auto transaction = fuse_core::Transaction::make_shared(); @@ -407,12 +364,9 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() - << ", x: " << position->x() << ", y: " - << position->y() << ", yaw: " << orientation->yaw() << - ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() << ", x: " + << position->x() << ", y: " << position->y() + << ", yaw: " << orientation->yaw() << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 9accc5061..51281873b 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -50,36 +50,24 @@ namespace fuse_models { Unicycle2DStateKinematicConstraint::Unicycle2DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & yaw1, - const fuse_variables::VelocityLinear2DStamped & linear_velocity1, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & yaw2, - const fuse_variables::VelocityLinear2DStamped & linear_velocity2, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration2, - const fuse_core::Matrix8d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), - yaw1.uuid(), - linear_velocity1.uuid(), - yaw_velocity1.uuid(), - linear_acceleration1.uuid(), - position2.uuid(), - yaw2.uuid(), - linear_velocity2.uuid(), - yaw_velocity2.uuid(), - linear_acceleration2.uuid()}), // NOLINT - dt_((position2.stamp() - position1.stamp()).seconds()), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& yaw1, const fuse_variables::VelocityLinear2DStamped& linear_velocity1, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, + const fuse_variables::Position2DStamped& position2, const fuse_variables::Orientation2DStamped& yaw2, + const fuse_variables::VelocityLinear2DStamped& linear_velocity2, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, const fuse_core::Matrix8d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), yaw1.uuid(), linear_velocity1.uuid(), yaw_velocity1.uuid(), + linear_acceleration1.uuid(), position2.uuid(), yaw2.uuid(), linear_velocity2.uuid(), + yaw_velocity2.uuid(), linear_acceleration2.uuid() }) + , // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void Unicycle2DStateKinematicConstraint::print(std::ostream & stream) const +void Unicycle2DStateKinematicConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -98,7 +86,7 @@ void Unicycle2DStateKinematicConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * Unicycle2DStateKinematicConstraint::costFunction() const +ceres::CostFunction* Unicycle2DStateKinematicConstraint::costFunction() const { // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: diff --git a/fuse_models/test/example_constraint.hpp b/fuse_models/test/example_constraint.hpp index 99d371f76..a33cdb666 100644 --- a/fuse_models/test/example_constraint.hpp +++ b/fuse_models/test/example_constraint.hpp @@ -49,7 +49,6 @@ #include #include - /** * @brief Dummy constraint implementation for testing */ @@ -60,33 +59,28 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint( - const std::string & source, - std::initializer_list variable_uuid_list) - : fuse_core::Constraint(source, variable_uuid_list), data(0.0) + ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } - template - ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), data(0.0) + template + ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(0.0) { } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " source: " << source() << '\n' << " uuid: " << uuid() << '\n' << " variables: ["; - const auto & variable_uuids = variables(); - if (!variable_uuids.empty()) { - std::copy( - variable_uuids.begin(), - variable_uuids.end() - 1, std::ostream_iterator(stream, ", ")); + const auto& variable_uuids = variables(); + if (!variable_uuids.empty()) + { + std::copy(variable_uuids.begin(), variable_uuids.end() - 1, std::ostream_iterator(stream, ", ")); stream << variable_uuids.back(); } @@ -94,7 +88,7 @@ class ExampleConstraint : public fuse_core::Constraint << " data: " << data << '\n'; } - ceres::CostFunction * costFunction() const override + ceres::CostFunction* costFunction() const override { return nullptr; } @@ -112,11 +106,11 @@ class ExampleConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_models/test/example_variable.hpp b/fuse_models/test/example_variable.hpp index 4eaa80e4b..182974429 100644 --- a/fuse_models/test/example_variable.hpp +++ b/fuse_models/test/example_variable.hpp @@ -52,8 +52,7 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - ExampleVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) + ExampleVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) { } @@ -62,17 +61,17 @@ class ExampleVariable : public fuse_core::Variable return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " uuid: " << uuid() << '\n' @@ -92,11 +91,11 @@ class ExampleVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_models/test/example_variable_stamped.hpp b/fuse_models/test/example_variable_stamped.hpp index ea04bd1fd..7768ba7e9 100644 --- a/fuse_models/test/example_variable_stamped.hpp +++ b/fuse_models/test/example_variable_stamped.hpp @@ -55,12 +55,10 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables ExampleVariableStamped() = default; - explicit ExampleVariableStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : fuse_core::Variable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id), - data_(0.0) + explicit ExampleVariableStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : fuse_core::Variable(fuse_core::uuid::generate(detail::type(), stamp, device_id)) + , Stamped(stamp, device_id) + , data_(0.0) { } @@ -69,17 +67,17 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " uuid: " << uuid() << '\n' @@ -101,12 +99,12 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index 2b06111ac..d4776b249 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -77,35 +77,38 @@ static std::string failure_description; // NOLINT(runtime/string) * @brief Compare all the properties of two Variable objects * @return True if all the properties match, false otherwise */ -bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Variable & actual) +bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) { failure_description = ""; bool variables_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { variables_equal = false; - failure_description += "The variables have different types.\n expected type is '" + - expected.type() + - "'\n actual type is '" + actual.type() + "'\n"; + failure_description += "The variables have different types.\n expected type is '" + expected.type() + + "'\n actual type is '" + actual.type() + "'\n"; } - if (expected.size() != actual.size()) { + if (expected.size() != actual.size()) + { variables_equal = false; failure_description += "The variables have different sizes.\n expected size is '" + - std::to_string(expected.size()) + "'\n actual size is '" + std::to_string(actual.size()) + - "'\n"; + std::to_string(expected.size()) + "'\n actual size is '" + std::to_string(actual.size()) + + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { variables_equal = false; failure_description += "The variables have different UUIDs.\n expected UUID is '" + - fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + - fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - for (size_t i = 0; i < expected.size(); ++i) { - if (expected.data()[i] != actual.data()[i]) { + for (size_t i = 0; i < expected.size(); ++i) + { + if (expected.data()[i] != actual.data()[i]) + { variables_equal = false; - failure_description += "The variables have different values.\n expected data(" + - std::to_string(i) + ") is '" + - std::to_string(expected.data()[i]) + "'\n actual data(" + std::to_string(i) + ") is '" + - std::to_string(actual.data()[i]) + "'\n"; + failure_description += "The variables have different values.\n expected data(" + std::to_string(i) + ") is '" + + std::to_string(expected.data()[i]) + "'\n actual data(" + std::to_string(i) + ") is '" + + std::to_string(actual.data()[i]) + "'\n"; } } return variables_equal; @@ -115,40 +118,40 @@ bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Var * @brief Compare all the properties of two Constraint objects * @return True if all the properties match, false otherwise */ -bool compareConstraints( - const fuse_core::Constraint & expected, - const fuse_core::Constraint & actual) +bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) { failure_description = ""; bool constraints_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { constraints_equal = false; - failure_description += "The constraints have different types.\n expected type is '" + - expected.type() + - "'\n actual type is '" + actual.type() + "'\n"; + failure_description += "The constraints have different types.\n expected type is '" + expected.type() + + "'\n actual type is '" + actual.type() + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { constraints_equal = false; failure_description += "The constraints have different UUIDs.\n expected UUID is '" + - fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + - fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - if (expected.variables().size() != actual.variables().size()) { + if (expected.variables().size() != actual.variables().size()) + { constraints_equal = false; - failure_description += - "The constraints involve a different number of variables.\n expected variable count is '" + - std::to_string(expected.variables().size()) + "'\n actual variable count is '" + - std::to_string(actual.variables().size()) + "'\n"; + failure_description += "The constraints involve a different number of variables.\n expected variable count is '" + + std::to_string(expected.variables().size()) + "'\n actual variable count is '" + + std::to_string(actual.variables().size()) + "'\n"; } - for (size_t i = 0; i < expected.variables().size(); ++i) { - if (expected.variables().at(i) != actual.variables().at(i)) { + for (size_t i = 0; i < expected.variables().size(); ++i) + { + if (expected.variables().at(i) != actual.variables().at(i)) + { constraints_equal = false; std::string i_str = std::to_string(i); - failure_description += - "The constraints involve different variable UUIDs.\n expected variables(" + i_str + - ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + - "'\n actual variables(" + i_str + ") is '" + - fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; + failure_description += "The constraints involve different variable UUIDs.\n expected variables(" + i_str + + ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + + "'\n actual variables(" + i_str + ") is '" + + fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; } } return constraints_equal; @@ -157,22 +160,22 @@ bool compareConstraints( namespace fuse_core { -bool operator==(const fuse_core::Variable & rhs, const fuse_core::Variable & lhs) +bool operator==(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) { return compareVariables(rhs, lhs); } -bool operator!=(const fuse_core::Variable & rhs, const fuse_core::Variable & lhs) +bool operator!=(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) { return !(rhs == lhs); } -bool operator==(const fuse_core::Constraint & rhs, const fuse_core::Constraint & lhs) +bool operator==(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) { return compareConstraints(rhs, lhs); } -bool operator!=(const fuse_core::Constraint & rhs, const fuse_core::Constraint & lhs) +bool operator!=(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) { return !(rhs == lhs); } @@ -190,23 +193,21 @@ class GraphIgnitionTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; }; @@ -214,11 +215,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.set_graph_service:=set_graph", - "-p", "ignition_sensor.reset_service:=''"}); + options.arguments({ "--ros-args", "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", + "ignition_sensor.reset_service:=''" }); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -247,14 +245,12 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable1->uuid(), variable2->uuid()}); // NOLINT + "test", std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable2->uuid(), variable3->uuid()}); // NOLINT + "test", std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT constraint2->data = -3.7; graph.addConstraint(constraint2); @@ -295,22 +291,30 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto & added_constraint : transaction->addedConstraints()) { - try { - const auto & constraint = graph.getConstraint(added_constraint.uuid()); + for (const auto& added_constraint : transaction->addedConstraints()) + { + try + { + const auto& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } - for (const auto & added_variable : transaction->addedVariables()) { - try { - const auto & variable = graph.getVariable(added_variable.uuid()); + for (const auto& added_variable : transaction->addedVariables()) + { + try + { + const auto& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } @@ -327,11 +331,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.set_graph_service:=set_graph", - "-p", "ignition_sensor.reset_service:=''"}); + options.arguments({ "--ros-args", "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", + "ignition_sensor.reset_service:=''" }); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -360,14 +361,12 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable1->uuid(), variable2->uuid()}); // NOLINT + "test", std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable2->uuid(), variable3->uuid()}); // NOLINT + "test", std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT constraint2->data = -3.7; graph.addConstraint(constraint2); @@ -408,22 +407,30 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto & added_constraint : transaction->addedConstraints()) { - try { - const auto & constraint = graph.getConstraint(added_constraint.uuid()); + for (const auto& added_constraint : transaction->addedConstraints()) + { + try + { + const auto& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } - for (const auto & added_variable : transaction->addedVariables()) { - try { - const auto & variable = graph.getVariable(added_variable.uuid()); + for (const auto& added_variable : transaction->addedVariables()) + { + try + { + const auto& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 637a93699..c61455768 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -14,54 +14,42 @@ namespace fm_common = fuse_models::common; TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) { - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXPositionAndYawOrientationIndices) { - const std::vector position_indices{0}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXYPositionAndEmptyOrientationIndices) { - const std::vector position_indices{0, 1}; + const std::vector position_indices{ 0, 1 }; const std::vector orientation_indices; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -70,13 +58,11 @@ TEST(TestSuite, mergeXYPositionAndEmptyOrientationIndices) TEST(TestSuite, mergeEmptyPositionAndYawOrientationIndices) { const std::vector position_indices; - const std::vector orientation_indices{0}; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -89,9 +75,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -104,24 +88,18 @@ TEST(TestSuite, populatePartialMeasurementXYPositionYawOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean, pose_mean_partial); EXPECT_EQ(pose_covariance, pose_covariance_partial); @@ -135,38 +113,33 @@ TEST(TestSuite, populatePartialMeasurementXPositionYawOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); // Eigen indexing is only supported in the latest stable versions, so we avoid using that feature // for backwards compatibility - const std::vector expected_merged_indices{0, 2}; + const std::vector expected_merged_indices{ 0, 2 }; EXPECT_THAT(expected_merged_indices, testing::ElementsAreArray(merged_indices)); - for (size_t row = 0; row < expected_merged_indices.size(); ++row) { + for (size_t row = 0; row < expected_merged_indices.size(); ++row) + { EXPECT_EQ(pose_mean(expected_merged_indices[row]), pose_mean_partial(row)); - for (size_t column = 0; column < expected_merged_indices.size(); ++column) { - EXPECT_EQ( - pose_covariance(expected_merged_indices[row], expected_merged_indices[column]), - pose_covariance_partial(row, column)); + for (size_t column = 0; column < expected_merged_indices.size(); ++column) + { + EXPECT_EQ(pose_covariance(expected_merged_indices[row], expected_merged_indices[column]), + pose_covariance_partial(row, column)); } } } @@ -180,23 +153,17 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionYawOrientation) pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; const std::vector position_indices; - const std::vector orientation_indices{0}; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean.tail<1>(), pose_mean_partial); EXPECT_EQ(pose_covariance.bottomRightCorner(1, 1), pose_covariance_partial); @@ -210,24 +177,18 @@ TEST(TestSuite, populatePartialMeasurementXYPositionEmptyOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0, 1}; + const std::vector position_indices{ 0, 1 }; const std::vector orientation_indices; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean.head<2>(), pose_mean_partial); EXPECT_EQ(pose_covariance.topLeftCorner(2, 2), pose_covariance_partial); @@ -246,19 +207,13 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionEmptyOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(0, pose_mean_partial.size()); EXPECT_EQ(0, pose_covariance_partial.size()); diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index a132a82f3..1d60e4b81 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -21,9 +21,9 @@ class Unicycle2DModelTest : public fuse_models::Unicycle2D { public: - using fuse_models::Unicycle2D::updateStateHistoryEstimates; - using fuse_models::Unicycle2D::StateHistoryElement; using fuse_models::Unicycle2D::StateHistory; + using fuse_models::Unicycle2D::StateHistoryElement; + using fuse_models::Unicycle2D::updateStateHistoryEstimates; }; TEST(Unicycle2D, UpdateStateHistoryEstimates) @@ -33,8 +33,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_velocity1 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(1, 0)); auto yaw_velocity1 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(1, 0)); - auto linear_acceleration1 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; yaw1->yaw() = 3.1; @@ -47,8 +46,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_velocity2 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(2, 0)); auto yaw_velocity2 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(2, 0)); - auto linear_acceleration2 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; yaw2->yaw() = M_PI / 2.0; @@ -61,8 +59,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_velocity3 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(3, 0)); auto yaw_velocity3 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(3, 0)); - auto linear_acceleration3 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; yaw3->yaw() = 3.3; @@ -75,8 +72,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_velocity4 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(4, 0)); auto yaw_velocity4 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(4, 0)); - auto linear_acceleration4 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; yaw4->yaw() = 3.4; @@ -89,8 +85,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw5 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_velocity5 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(5, 0)); auto yaw_velocity5 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(5, 0)); - auto linear_acceleration5 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; yaw5->yaw() = 3.5; @@ -116,71 +111,39 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) // Add all of the variables to the state history Unicycle2DModelTest::StateHistory state_history; - state_history.emplace( - position1->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position1->uuid(), - yaw1->uuid(), - linear_velocity1->uuid(), - yaw_velocity1->uuid(), - linear_acceleration1->uuid(), - tf2_2d::Transform(1.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position2->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position2->uuid(), - yaw2->uuid(), - linear_velocity2->uuid(), - yaw_velocity2->uuid(), - linear_acceleration2->uuid(), - tf2_2d::Transform(2.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position3->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position3->uuid(), - yaw3->uuid(), - linear_velocity3->uuid(), - yaw_velocity3->uuid(), - linear_acceleration3->uuid(), - tf2_2d::Transform(3.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position4->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position4->uuid(), - yaw4->uuid(), - linear_velocity4->uuid(), - yaw_velocity4->uuid(), - linear_acceleration4->uuid(), - tf2_2d::Transform(4.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position5->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position5->uuid(), - yaw5->uuid(), - linear_velocity5->uuid(), - yaw_velocity5->uuid(), - linear_acceleration5->uuid(), - tf2_2d::Transform(5.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace(position1->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position1->uuid(), yaw1->uuid(), linear_velocity1->uuid(), yaw_velocity1->uuid(), + linear_acceleration1->uuid(), tf2_2d::Transform(1.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position2->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position2->uuid(), yaw2->uuid(), linear_velocity2->uuid(), yaw_velocity2->uuid(), + linear_acceleration2->uuid(), tf2_2d::Transform(2.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position3->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position3->uuid(), yaw3->uuid(), linear_velocity3->uuid(), yaw_velocity3->uuid(), + linear_acceleration3->uuid(), tf2_2d::Transform(3.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position4->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position4->uuid(), yaw4->uuid(), linear_velocity4->uuid(), yaw_velocity4->uuid(), + linear_acceleration4->uuid(), tf2_2d::Transform(4.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position5->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position5->uuid(), yaw5->uuid(), linear_velocity5->uuid(), yaw_velocity5->uuid(), + linear_acceleration5->uuid(), tf2_2d::Transform(5.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) // Update the state history - Unicycle2DModelTest::updateStateHistoryEstimates( - graph, state_history, rclcpp::Duration::from_seconds( - 10.0)); + Unicycle2DModelTest::updateStateHistoryEstimates(graph, state_history, rclcpp::Duration::from_seconds(10.0)); // Check the state estimates in the state history { @@ -274,9 +237,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) { // The fifth entry is missing from the graph. It will get predicted from previous state. // These values were verified with Octave - auto expected_pose = tf2_2d::Transform( - -3.9778707804360529, -8.9511455751801616, - -2.7663706143591722); + auto expected_pose = tf2_2d::Transform(-3.9778707804360529, -8.9511455751801616, -2.7663706143591722); auto actual_pose = state_history[rclcpp::Time(5, 0)].pose; EXPECT_NEAR(expected_pose.x(), actual_pose.x(), 1.0e-9); EXPECT_NEAR(expected_pose.y(), actual_pose.y(), 1.0e-9); diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index 4f799a3e2..8122e8513 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -49,12 +49,11 @@ #include #include -using fuse_constraints::AbsolutePosition2DStampedConstraint; +using fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint; using fuse_constraints::AbsoluteOrientation2DStampedConstraint; -using fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint; +using fuse_constraints::AbsolutePosition2DStampedConstraint; using fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint; -using fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint; - +using fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint; /** * @brief Promise used to communicate between the tests and the callback @@ -72,19 +71,20 @@ void transactionCallback(fuse_core::Transaction::SharedPtr transaction) /** * @brief Helper function for fetching the desired constraint from a transaction */ -template -const Derived * getConstraint(const fuse_core::Transaction & transaction) +template +const Derived* getConstraint(const fuse_core::Transaction& transaction) { - for (const auto & constraint : transaction.addedConstraints()) { - auto derived = dynamic_cast(&constraint); - if (derived) { + for (const auto& constraint : transaction.addedConstraints()) + { + auto derived = dynamic_cast(&constraint); + if (derived) + { return derived; } } return nullptr; } - class Unicycle2DIgnitionTestFixture : public ::testing::Test { public: @@ -96,23 +96,21 @@ class Unicycle2DIgnitionTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; }; @@ -120,13 +118,12 @@ TEST_F(Unicycle2DIgnitionTestFixture, InitialTransaction) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -201,14 +198,13 @@ TEST_F(Unicycle2DIgnitionTestFixture, SkipInitialTransaction) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -230,16 +226,14 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_service:=set_pose", - "-p", "ignition_sensor.reset_service:=''", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_service:=set_pose", "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -331,16 +325,14 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", - "-p", "ignition_sensor.reset_service:=''", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", "-p", + "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -363,8 +355,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) srv->pose.pose.covariance[0] = 1.0; srv->pose.pose.covariance[7] = 2.0; srv->pose.pose.covariance[35] = 3.0; - auto client = node->create_client( - "/unicycle_2d_ignition_test/set_pose_deprecated"); + auto client = + node->create_client("/unicycle_2d_ignition_test/set_pose_deprecated"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); diff --git a/fuse_models/test/test_unicycle_2d_predict.cpp b/fuse_models/test/test_unicycle_2d_predict.cpp index 1b3fc57b2..d57547005 100644 --- a/fuse_models/test/test_unicycle_2d_predict.cpp +++ b/fuse_models/test/test_unicycle_2d_predict.cpp @@ -61,24 +61,9 @@ TEST(Predict, predictDirectVals) double acc_linear2_x = 0.0; double acc_linear2_y = 0.0; - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.105, position2_x); EXPECT_DOUBLE_EQ(0.0, position2_y); @@ -90,24 +75,9 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2_y); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, acc_linear2_x, + acc_linear2_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.21858415916807189, position2_x); EXPECT_DOUBLE_EQ(0.017989963481956205, position2_y); @@ -123,24 +93,9 @@ TEST(Predict, predictDirectVals) vel_yaw1 = -1.570796327; acc_linear1_y = -1.0; - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.105, position2_x); EXPECT_DOUBLE_EQ(-0.105, position2_y); @@ -169,18 +124,8 @@ TEST(Predict, predictPointers) double vel_yaw2 = 1.570796327; std::vector acc_linear2(2, 0.0); - fuse_models::predict( - position1.data(), - &yaw1, - vel_linear1.data(), - &vel_yaw1, - acc_linear1.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position1.data(), &yaw1, vel_linear1.data(), &vel_yaw1, acc_linear1.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(0.0, position2[1]); @@ -192,18 +137,8 @@ TEST(Predict, predictPointers) EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position2.data(), &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); @@ -219,18 +154,8 @@ TEST(Predict, predictPointers) vel_yaw1 = -1.570796327; acc_linear1[1] = -1.0; - fuse_models::predict( - position1.data(), - &yaw1, - vel_linear1.data(), - &vel_yaw1, - acc_linear1.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position1.data(), &yaw1, vel_linear1.data(), &vel_yaw1, acc_linear1.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(-0.105, position2[1]); @@ -256,16 +181,7 @@ TEST(Predict, predictObjects) double vel_yaw2 = 0.0; tf2_2d::Vector2 acc_linear2; - fuse_models::predict( - pose1, - vel_linear1, - vel_yaw1, - acc_linear1, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); + fuse_models::predict(pose1, vel_linear1, vel_yaw1, acc_linear1, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, pose2.x()); EXPECT_DOUBLE_EQ(0.0, pose2.y()); @@ -277,17 +193,7 @@ TEST(Predict, predictObjects) EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - pose2, - vel_linear2, - vel_yaw2, - acc_linear2, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); - + fuse_models::predict(pose2, vel_linear2, vel_yaw2, acc_linear2, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.21858415916807189, pose2.x()); EXPECT_DOUBLE_EQ(0.017989963481956205, pose2.y()); @@ -303,16 +209,7 @@ TEST(Predict, predictObjects) vel_yaw1 = -1.570796327; acc_linear1.setY(-1.0); - fuse_models::predict( - pose1, - vel_linear1, - vel_yaw1, - acc_linear1, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); + fuse_models::predict(pose1, vel_linear1, vel_yaw1, acc_linear1, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, pose2.x()); EXPECT_DOUBLE_EQ(-0.105, pose2.y()); @@ -344,38 +241,23 @@ TEST(Predict, predictJacobians) double acc_linear2_x = 0.0; double acc_linear2_y = 0.0; - const std::array block_sizes = {2, 1, 2, 1, 2}; + const std::array block_sizes = { 2, 1, 2, 1, 2 }; const auto num_parameter_blocks = block_sizes.size(); - const size_t num_residuals{8}; + const size_t num_residuals{ 8 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y, - jacobians.data()); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y, jacobians.data()); fuse_core::Matrix8d J_analytic; J_analytic << J[0], J[1], J[2], J[3], J[4]; @@ -410,41 +292,20 @@ TEST(Predict, predictJacobians) Jet jet_acc_linear2_x(acc_linear2_x, 6); Jet jet_acc_linear2_y(acc_linear2_y, 7); - fuse_models::predict( - jet_position1_x, - jet_position1_y, - jet_yaw1, - jet_vel_linear1_x, - jet_vel_linear1_y, - jet_vel_yaw1, - jet_acc_linear1_x, - jet_acc_linear1_y, - jet_dt, - jet_position2_x, - jet_position2_y, - jet_yaw2, - jet_vel_linear2_x, - jet_vel_linear2_y, - jet_vel_yaw2, - jet_acc_linear2_x, - jet_acc_linear2_y); + fuse_models::predict(jet_position1_x, jet_position1_y, jet_yaw1, jet_vel_linear1_x, jet_vel_linear1_y, jet_vel_yaw1, + jet_acc_linear1_x, jet_acc_linear1_y, jet_dt, jet_position2_x, jet_position2_y, jet_yaw2, + jet_vel_linear2_x, jet_vel_linear2_y, jet_vel_yaw2, jet_acc_linear2_x, jet_acc_linear2_y); fuse_core::Matrix8d J_autodiff; - J_autodiff << jet_position2_x.v, - jet_position2_y.v, - jet_yaw2.v, - jet_vel_linear2_x.v, - jet_vel_linear2_y.v, - jet_vel_yaw2.v, - jet_acc_linear2_x.v, - jet_acc_linear2_y.v; + J_autodiff << jet_position2_x.v, jet_position2_y.v, jet_yaw2.v, jet_vel_linear2_x.v, jet_vel_linear2_y.v, + jet_vel_yaw2.v, jet_acc_linear2_x.v, jet_acc_linear2_y.v; J_autodiff.transposeInPlace(); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); EXPECT_MATRIX_NEAR(J_autodiff, J_analytic, std::numeric_limits::epsilon()) - << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt) - << "\nAnalytic Jacobian =\n" << J_analytic.format(HeavyFmt); + << "Autodiff Jacobian =\n" + << J_autodiff.format(HeavyFmt) << "\nAnalytic Jacobian =\n" + << J_analytic.format(HeavyFmt); } 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 ac3202d40..c548d44a8 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -47,44 +47,42 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9}; + const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); - const double dt{0.1}; - const fuse_core::Matrix8d sqrt_information{covariance.inverse().llt().matrixU()}; + const double dt{ 0.1 }; + const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() }; - const fuse_models::Unicycle2DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[] = {0.0, 0.0}; - const double yaw1[] = {0.0}; - const double vel_linear1[] = {1.0, 0.0}; - const double vel_yaw1[] = {1.570796327}; - const double acc_linear1[] = {1.0, 0.0}; - - const double position2[] = {0.105, 0.0}; - const double yaw2[] = {0.1570796327}; - const double vel_linear2[] = {1.1, 0.0}; - const double vel_yaw2[] = {1.570796327}; - const double acc_linear2[] = {1.0, 0.0}; - - const double * parameters[] = - { - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 - }; + const double position1[] = { 0.0, 0.0 }; + const double yaw1[] = { 0.0 }; + const double vel_linear1[] = { 1.0, 0.0 }; + const double vel_yaw1[] = { 1.570796327 }; + const double acc_linear1[] = { 1.0, 0.0 }; + + const double position2[] = { 0.105, 0.0 }; + const double yaw2[] = { 0.1570796327 }; + const double vel_linear2[] = { 1.1, 0.0 }; + const double vel_yaw2[] = { 1.570796327 }; + const double acc_linear2[] = { 1.0, 0.0 }; + + const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; fuse_core::Vector8d residuals; - const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto& block_sizes = cost_function.parameter_block_sizes(); const auto num_parameter_blocks = block_sizes.size(); const auto num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -109,30 +107,28 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); - std::vector jacobians_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J_autodiff[i].resize(num_residuals, block_sizes[i]); jacobians_autodiff[i] = J_autodiff[i].data(); } - EXPECT_TRUE( - cost_function_autodiff.Evaluate( - parameters, residuals.data(), - jacobians_autodiff.data())); + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index e480aac28..d3d905322 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_optimizers { @@ -112,10 +111,8 @@ class BatchOptimizer : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - BatchOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ); + BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -131,11 +128,10 @@ class BatchOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - TransactionQueueElement( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) - : sensor_name(sensor_name), - transaction(std::move(transaction)) {} + TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) + : sensor_name(sensor_name), transaction(std::move(transaction)) + { + } }; /** @@ -151,24 +147,24 @@ class BatchOptimizer : public Optimizer //!< constraints and variables from //!< multiple sensors and motions models //!< before being applied to the graph. - std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction - //!< across different threads - ParameterType params_; //!< Configuration settings for this optimizer - std::atomic optimization_request_; //!< Flag to trigger a new optimization - std::condition_variable optimization_requested_; //!< Condition variable used by the optimization - //!< thread to wait until a new optimization is - //!< requested by the main thread - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction + //!< across different threads + ParameterType params_; //!< Configuration settings for this optimizer + std::atomic optimization_request_; //!< Flag to trigger a new optimization + std::condition_variable optimization_requested_; //!< Condition variable used by the optimization + //!< thread to wait until a new optimization is + //!< requested by the main thread + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed //!< frequency - TransactionQueue pending_transactions_; //!< The set of received transactions that have not been - //!< added to the optimizer yet. Transactions are added - //!< by the main thread, and removed and processed by the - //!< optimization thread. - std::mutex pending_transactions_mutex_; //!< Synchronize modification of the - //!< pending_transactions_ container - rclcpp::Time start_time_; //!< The timestamp of the first ignition sensor transaction + TransactionQueue pending_transactions_; //!< The set of received transactions that have not been + //!< added to the optimizer yet. Transactions are added + //!< by the main thread, and removed and processed by the + //!< optimization thread. + std::mutex pending_transactions_mutex_; //!< Synchronize modification of the + //!< pending_transactions_ container + rclcpp::Time start_time_; //!< The timestamp of the first ignition sensor transaction bool started_; //!< Flag indicating the optimizer is ready/has received a transaction from an //!< ignition sensor @@ -215,15 +211,13 @@ class BatchOptimizer : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp index ba4011db3..5d64d5f9f 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_optimizers { @@ -63,7 +62,7 @@ struct BatchOptimizerParams * or in the "optimization_frequency" parameter in Hz. "optimization_frequency" will be * prioritized. */ - rclcpp::Duration optimization_period {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration optimization_period{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -72,7 +71,7 @@ struct BatchOptimizerParams * while waiting for motion models to be generated. Once the timeout expires, that transaction * will be deleted from the queue. */ - rclcpp::Duration transaction_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration transaction_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -85,28 +84,24 @@ struct BatchOptimizerParams * @param[in] interfaces - The node interfaces used to load the parameter */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces) + fuse_core::node_interfaces::NodeInterfaces + interfaces) { // Read settings from the parameter server - double optimization_frequency{-1.0}; - optimization_frequency = fuse_core::getParam( - interfaces, "optimization_frequency", - optimization_frequency); + double optimization_frequency{ -1.0 }; + optimization_frequency = fuse_core::getParam(interfaces, "optimization_frequency", optimization_frequency); fuse_core::getPositiveParam(interfaces, "optimization_period", optimization_period); - if (optimization_frequency != -1.0) { - if (optimization_frequency < 0) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested optimization_frequency parameter is < 0. Using the optimization_period" - "parameter instead!"); + if (optimization_frequency != -1.0) + { + if (optimization_frequency < 0) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested optimization_frequency parameter is < 0. Using the optimization_period" + "parameter instead!"); } - optimization_period = - rclcpp::Duration::from_seconds(1.0 / optimization_frequency); + optimization_period = rclcpp::Duration::from_seconds(1.0 / optimization_frequency); } fuse_core::getPositiveParam(interfaces, "transaction_timeout", transaction_timeout); diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index da0a61930..53a07f651 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -55,7 +55,6 @@ #include #include - namespace fuse_optimizers { @@ -125,10 +124,8 @@ class FixedLagSmoother : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - FixedLagSmoother( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ); + FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -144,9 +141,18 @@ class FixedLagSmoother : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - const rclcpp::Time & stamp() const {return transaction->stamp();} - const rclcpp::Time & minStamp() const {return transaction->minStamp();} - const rclcpp::Time & maxStamp() const {return transaction->maxStamp();} + const rclcpp::Time& stamp() const + { + return transaction->stamp(); + } + const rclcpp::Time& minStamp() const + { + return transaction->minStamp(); + } + const rclcpp::Time& maxStamp() const + { + return transaction->maxStamp(); + } }; /** @@ -163,15 +169,15 @@ class FixedLagSmoother : public Optimizer // Read-only after construction std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process - ParameterType params_; //!< Configuration settings for this fixed-lag smoother + ParameterType params_; //!< Configuration settings for this fixed-lag smoother // Inherently thread-safe - std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an - //!< ignition sensor and it is queued but not processed yet + std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an + //!< ignition sensor and it is queued but not processed yet std::atomic optimization_running_; //!< Flag indicating the optimization thread should be //!< running - std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an - //!< ignition sensor + std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an + //!< ignition sensor // Guarded by pending_transactions_mutex_ std::mutex pending_transactions_mutex_; //!< Synchronize modification of the @@ -184,27 +190,27 @@ class FixedLagSmoother : public Optimizer // Guarded by optimization_mutex_ std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized // fuse_core::Graph* graph_ member from the base class - rclcpp::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window + rclcpp::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next //!< optimization cycle - VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with - //!< each variable - ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read - //!< by setDiagnostics + VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with + //!< each variable + ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read + //!< by setDiagnostics // Guarded by optimization_requested_mutex_ - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - rclcpp::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers - //!< a warning if exceeded. - bool optimization_request_; //!< Flag to trigger a new optimization + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + rclcpp::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers + //!< a warning if exceeded. + bool optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization //!< thread to wait until a new optimization is //!< requested by the main thread // Guarded by start_time_mutex_ - mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable - rclcpp::Time start_time_ {0, 0, RCL_ROS_TIME}; //!< The timestamp of the first ignition sensor - //!< transaction + mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable + rclcpp::Time start_time_{ 0, 0, RCL_ROS_TIME }; //!< The timestamp of the first ignition sensor + //!< transaction // Ordering ROS objects with callbacks last rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed @@ -229,7 +235,7 @@ class FixedLagSmoother : public Optimizer * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to * the graph */ - void preprocessMarginalization(const fuse_core::Transaction & new_transaction); + void preprocessMarginalization(const fuse_core::Transaction& new_transaction); /** * @brief Compute the oldest timestamp that is part of the configured lag window @@ -246,7 +252,7 @@ class FixedLagSmoother : public Optimizer * @return A container with the set of variables to marginalize out. Order of the variables is not * specified. */ - std::vector computeVariablesToMarginalize(const rclcpp::Time & lag_expiration); + std::vector computeVariablesToMarginalize(const rclcpp::Time& lag_expiration); /** * @brief Perform any required post-marginalization bookkeeping @@ -257,7 +263,7 @@ class FixedLagSmoother : public Optimizer * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the * requested variables. */ - void postprocessMarginalization(const fuse_core::Transaction & marginal_transaction); + void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction); /** * @brief Function that optimizes all constraints, designed to be run in a separate thread. @@ -290,15 +296,13 @@ class FixedLagSmoother : public Optimizer * sensor transactions * @param[in] lag_expiration The oldest timestamp that should remain in the graph */ - void processQueue(fuse_core::Transaction & transaction, const rclcpp::Time & lag_expiration); + void processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration); /** * @brief Service callback that resets the optimizer to its original state */ - bool resetServiceCallback( - const std::shared_ptr, - std::shared_ptr - ); + bool resetServiceCallback(const std::shared_ptr, + std::shared_ptr); /** * @brief Thread-safe read-only access to the timestamp of the first transaction @@ -312,7 +316,7 @@ class FixedLagSmoother : public Optimizer /** * @brief Thread-safe write access to the optimizer start time */ - void setStartTime(const rclcpp::Time & start_time) + void setStartTime(const rclcpp::Time& start_time) { std::lock_guard lock(start_time_mutex_); start_time_ = start_time; @@ -331,15 +335,13 @@ class FixedLagSmoother : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index fadba05d7..735b6e3cc 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ #define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_optimizers/fixed_lag_smoother_params.hpp instead +#warning This header is obsolete, please include fuse_optimizers/fixed_lag_smoother_params.hpp instead #include diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp index 139261ca5..ee967cc54 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_optimizers { @@ -58,7 +57,7 @@ struct FixedLagSmootherParams /** * @brief The duration of the smoothing window in seconds */ - rclcpp::Duration lag_duration {5, 0}; + rclcpp::Duration lag_duration{ 5, 0 }; /** * @brief The target duration for optimization cycles @@ -68,12 +67,12 @@ struct FixedLagSmootherParams * or in the "optimization_frequency" parameter in Hz. "optimization_frequency" will be * prioritized. */ - rclcpp::Duration optimization_period {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration optimization_period{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief The topic name of the advertised reset service */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -82,7 +81,7 @@ struct FixedLagSmootherParams * while waiting for motion models to be generated. Once the timeout expires, that transaction * will be deleted from the queue. */ - rclcpp::Duration transaction_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration transaction_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -96,30 +95,26 @@ struct FixedLagSmootherParams * @param[in] node - The node used to load the parameter */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces) + fuse_core::node_interfaces::NodeInterfaces + interfaces) { // Read settings from the parameter server fuse_core::getPositiveParam(interfaces, "lag_duration", lag_duration); - double optimization_frequency{-1.0}; - optimization_frequency = fuse_core::getParam( - interfaces, "optimization_frequency", - optimization_frequency); + double optimization_frequency{ -1.0 }; + optimization_frequency = fuse_core::getParam(interfaces, "optimization_frequency", optimization_frequency); fuse_core::getPositiveParam(interfaces, "optimization_period", optimization_period); - if (optimization_frequency != -1.0) { - if (optimization_frequency < 0) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested optimization_frequency parameter is < 0. Using the optimization_period" - "parameter instead!"); + if (optimization_frequency != -1.0) + { + if (optimization_frequency < 0) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested optimization_frequency parameter is < 0. Using the optimization_period" + "parameter instead!"); } - optimization_period = - rclcpp::Duration::from_seconds(1.0 / optimization_frequency); + optimization_period = rclcpp::Duration::from_seconds(1.0 / optimization_frequency); } fuse_core::getParam(interfaces, "reset_service", reset_service); diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index 71d36f279..54ede9a92 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_optimizers { @@ -104,10 +103,8 @@ class Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - Optimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ); + Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -134,8 +131,7 @@ class Optimizer * @param[in] model The sensor model * @param[in] ignition Whether this sensor model is an ignition one or not */ - SensorModelInfo(SensorModelUniquePtr model, const bool ignition) - : model(std::move(model)), ignition(ignition) + SensorModelInfo(SensorModelUniquePtr model, const bool ignition) : model(std::move(model)), ignition(ignition) { } @@ -157,12 +153,12 @@ class Optimizer AssociatedMotionModels associated_motion_models_; //!< Tracks what motion models should be used //!< for each sensor - fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and - //!< constraints + fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and + //!< constraints pluginlib::ClassLoader motion_model_loader_; //!< Pluginlib class loader //!< for MotionModels - MotionModels motion_models_; //!< The set of motion models, addressable by name + MotionModels motion_models_; //!< The set of motion models, addressable by name pluginlib::ClassLoader publisher_loader_; //!< Pluginlib class loader for //!< Publishers Publishers publishers_; //!< The set of publishers to execute after every graph optimization @@ -174,7 +170,6 @@ class Optimizer std::shared_ptr callback_queue_; - /** * @brief Callback fired every time a SensorModel plugin creates a new transaction * @@ -184,9 +179,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - virtual void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) = 0; + virtual void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; /** * @brief Configure the motion model plugins specified on the parameter server @@ -225,9 +218,7 @@ class Optimizer * @return Flag indicating if all motion model constraints were successfully * generated */ - bool applyMotionModels( - const std::string & sensor_name, - fuse_core::Transaction & transaction) const; + bool applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const; /** * @brief Send the sensors, motion models, and publishers updated graph information @@ -236,9 +227,7 @@ class Optimizer * removals * @param[in] graph A read-only pointer to the graph object */ - void notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph); + void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); /** * @brief Inject a transaction callback function into the global callback queue @@ -247,9 +236,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void injectCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction); + void injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction); /** * @brief Clear all of the callbacks inserted into the callback queue by the injectCallback() @@ -271,7 +258,7 @@ class Optimizer * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status); + virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp index 2a85d2a75..ab166db75 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp @@ -44,7 +44,6 @@ #include - namespace fuse_optimizers { /** @@ -74,12 +73,18 @@ class VariableStampIndex /** * @brief Return true if no variables exist in the index */ - bool empty() const {return variables_.empty() && constraints_.empty();} + bool empty() const + { + return variables_.empty() && constraints_.empty(); + } /** * @brief Returns the number of variables in the index */ - size_t size() const {return variables_.size();} + size_t size() const + { + return variables_.size(); + } /** * @brief Clear all tracked state @@ -103,7 +108,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to add and remove */ - void addNewTransaction(const fuse_core::Transaction & transaction); + void addNewTransaction(const fuse_core::Transaction& transaction); /** * @brief Update the index with the information from a marginal transaction @@ -113,7 +118,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to remove */ - void addMarginalTransaction(const fuse_core::Transaction & transaction); + void addMarginalTransaction(const fuse_core::Transaction& transaction); /** * @brief Add all variables that are not directly connected to a stamped variable with a timestamp @@ -123,30 +128,37 @@ class VariableStampIndex * greater than or equal to this will be added to the output container * @param[out] result An output iterator capable of receiving fuse_core::UUID objects */ - template - void query(const rclcpp::Time & stamp, OutputUuidIterator result) const + template + void query(const rclcpp::Time& stamp, OutputUuidIterator result) const { // First get all of the stamped variables greater than or equal to the input stamp std::unordered_set recent_variable_uuids; - for (const auto & variable_stamp_pair : stamped_index_) { - if (variable_stamp_pair.second >= stamp) { + for (const auto& variable_stamp_pair : stamped_index_) + { + if (variable_stamp_pair.second >= stamp) + { recent_variable_uuids.insert(variable_stamp_pair.first); } } // Now find all of the variables connected to the recent variables std::unordered_set connected_variable_uuids; - for (const auto & recent_variable_uuid : recent_variable_uuids) { + for (const auto& recent_variable_uuid : recent_variable_uuids) + { // Add the recent variable to ensure connected_variable_uuids is a superset of // recent_variable_uuids connected_variable_uuids.insert(recent_variable_uuid); const auto variables_iter = variables_.find(recent_variable_uuid); - if (variables_iter != variables_.end()) { - for (const auto & connected_constraint_uuid : variables_iter->second) { + if (variables_iter != variables_.end()) + { + for (const auto& connected_constraint_uuid : variables_iter->second) + { const auto constraints_iter = constraints_.find(connected_constraint_uuid); - if (constraints_iter != constraints_.end()) { - for (const auto & connected_variable_uuid : constraints_iter->second) { + if (constraints_iter != constraints_.end()) + { + for (const auto& connected_variable_uuid : constraints_iter->second) + { connected_variable_uuids.insert(connected_variable_uuid); } } @@ -155,8 +167,10 @@ class VariableStampIndex } // Return the set of variables that are not connected - for (const auto & variable : variables_) { - if (connected_variable_uuids.find(variable.first) == connected_variable_uuids.end()) { + for (const auto& variable : variables_) + { + if (connected_variable_uuids.find(variable.first) == connected_variable_uuids.end()) + { *result = variable.first; ++result; } @@ -168,34 +182,32 @@ class VariableStampIndex StampedMap stamped_index_; //!< Container that holds the UUID->Stamp mapping for //!< fuse_variables::Stamped variables - using VariableToConstraintsMap = std::unordered_map>; + using VariableToConstraintsMap = std::unordered_map>; VariableToConstraintsMap variables_; - using ConstraintToVariablesMap = std::unordered_map>; + using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction */ - void applyAddedConstraints(const fuse_core::Transaction & transaction); + void applyAddedConstraints(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the added variables from the provided transaction */ - void applyAddedVariables(const fuse_core::Transaction & transaction); + void applyAddedVariables(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the removed constraints from the provided * transaction */ - void applyRemovedConstraints(const fuse_core::Transaction & transaction); + void applyRemovedConstraints(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the removed variables from the provided transaction */ - void applyRemovedVariables(const fuse_core::Transaction & transaction); + void applyRemovedVariables(const fuse_core::Transaction& transaction); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index a4b520706..c87fa4b28 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -43,30 +43,23 @@ #include #include - namespace fuse_optimizers { -BatchOptimizer::BatchOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: fuse_optimizers::Optimizer(interfaces, std::move(graph)), - combined_transaction_(fuse_core::Transaction::make_shared()), - optimization_request_(false), - start_time_(rclcpp::Time::max()), - started_(false) +BatchOptimizer::BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + , combined_transaction_(fuse_core::Transaction::make_shared()) + , optimization_request_(false) + , start_time_(rclcpp::Time::max()) + , started_(false) { params_.loadFromROS(interfaces_); // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&BatchOptimizer::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + optimize_timer_ = rclcpp::create_timer(interfaces_, clock_, params_.optimization_period, + std::bind(&BatchOptimizer::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group()); // Start the optimization thread optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); @@ -77,7 +70,8 @@ BatchOptimizer::~BatchOptimizer() // Wake up any sleeping threads optimization_requested_.notify_all(); // Wait for the threads to shutdown - if (optimization_thread_.joinable()) { + if (optimization_thread_.joinable()) + { optimization_thread_.join(); } } @@ -88,29 +82,34 @@ void BatchOptimizer::applyMotionModelsToQueue() std::lock_guard pending_transactions_lock(pending_transactions_mutex_); rclcpp::Time current_time; // Use the most recent transaction time as the current time - if (!pending_transactions_.empty()) { + if (!pending_transactions_.empty()) + { // Use the most recent transaction time as the current time current_time = pending_transactions_.rbegin()->first; } // TODO(CH3): We might have to check for time validity here? Attempt to process each pending // transaction - while (!pending_transactions_.empty()) { - auto & element = pending_transactions_.begin()->second; + while (!pending_transactions_.empty()) + { + auto& element = pending_transactions_.begin()->second; // Apply the motion models to the transaction - if (!applyMotionModels(element.sensor_name, *element.transaction)) { - if (element.transaction->stamp() + params_.transaction_timeout < current_time) { + if (!applyMotionModels(element.sensor_name, *element.transaction)) + { + if (element.transaction->stamp() + params_.transaction_timeout < current_time) + { // Warn that this transaction has expired, then skip it. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.transaction->stamp().nanoseconds() << " could not be processed after " - << (current_time - element.transaction->stamp()).nanoseconds() - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); + RCLCPP_ERROR_STREAM(logger_, + "The queued transaction with timestamp " + << element.transaction->stamp().nanoseconds() << " could not be processed after " + << (current_time - element.transaction->stamp()).nanoseconds() + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); pending_transactions_.erase(pending_transactions_.begin()); continue; - } else { + } + else + { // Stop processing future transactions. Try again next time. break; } @@ -129,23 +128,20 @@ void BatchOptimizer::applyMotionModelsToQueue() void BatchOptimizer::optimizationLoop() { // Optimize constraints until told to exit - while (interfaces_.get_node_base_interface()->get_context()->is_valid()) { + while (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { // Wait for the next signal to start the next optimization cycle { std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait( - lock, - [this] { - /* *INDENT-OFF* */ - return ( - optimization_request_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid() - ); - /* *INDENT-ON* */ - }); + optimization_requested_.wait(lock, [this] { + /* *INDENT-OFF* */ + return (optimization_request_ || !interfaces_.get_node_base_interface()->get_context()->is_valid()); + /* *INDENT-ON* */ + }); } // If a shutdown is requested, exit now. - if (!interfaces_.get_node_base_interface()->get_context()->is_valid()) { + if (!interfaces_.get_node_base_interface()->get_context()->is_valid()) + { break; } // Copy the combined transaction so it can be shared with all the plugins @@ -171,7 +167,8 @@ void BatchOptimizer::optimizationLoop() void BatchOptimizer::optimizerTimerCallback() { // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) { + if (!started_) + { return; } // Attempt to generate motion models for any queued transactions @@ -184,14 +181,13 @@ void BatchOptimizer::optimizerTimerCallback() // If there is some pending work, trigger the next optimization cycle. // If the optimizer has not completed the previous optimization cycle, then it // will not be waiting on the condition variable signal, so nothing will happen. - if (optimization_request_) { + if (optimization_request_) + { optimization_requested_.notify_one(); } } -void BatchOptimizer::transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // Add the new transaction to the pending set // Either we haven't "started" yet and we want to keep a short history of transactions around @@ -200,28 +196,29 @@ void BatchOptimizer::transactionCallback( rclcpp::Time transaction_time = transaction->stamp(); rclcpp::Time last_pending_time(0, 0, transaction_clock_type); // NOTE(CH3): Uninitialized - if (!started_ || transaction_time >= start_time_) { + if (!started_ || transaction_time >= start_time_) + { std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.emplace( - transaction_time, - TransactionQueueElement(sensor_name, std::move(transaction))); + pending_transactions_.emplace(transaction_time, TransactionQueueElement(sensor_name, std::move(transaction))); last_pending_time = pending_transactions_.rbegin()->first; } // If we haven't "started" yet... - if (!started_) { + if (!started_) + { // Check if this transaction "starts" the system - if (sensor_models_.at(sensor_name).ignition) { + if (sensor_models_.at(sensor_name).ignition) + { started_ = true; start_time_ = transaction_time; } // Purge old transactions from the pending queue rclcpp::Time purge_time(0, 0, transaction_clock_type); // NOTE(CH3): Uninitialized - if (started_) { + if (started_) + { purge_time = start_time_; - } else if ( // prevent a bad subtraction // NOLINT - rclcpp::Time( - params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type() - ) < last_pending_time) + } + else if ( // prevent a bad subtraction // NOLINT + rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < last_pending_time) { purge_time = last_pending_time - params_.transaction_timeout; } @@ -230,12 +227,13 @@ void BatchOptimizer::transactionCallback( pending_transactions_.erase(pending_transactions_.begin(), purge_iter); } // If we have "started", attempt to process any pending transactions - if (started_) { + if (started_) + { applyMotionModelsToQueue(); } } -void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "BatchOptimizer"); diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp index e7d00b85f..efc2d3c14 100644 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ b/fuse_optimizers/src/batch_optimizer_node.cpp @@ -35,7 +35,7 @@ #include #include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("batch_optimizer_node"); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 87de2e8fe..36e22e6f7 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -58,10 +58,9 @@ namespace * @param[in] position A reverse iterator that access the element to be erased * @return A reverse iterator pointing to the element after the erased element */ -template -typename std::vector::reverse_iterator erase( - std::vector & container, - typename std::vector::reverse_iterator position) +template +typename std::vector::reverse_iterator erase(std::vector& container, + typename std::vector::reverse_iterator position) { // Reverse iterators are weird // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator @@ -78,15 +77,13 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { -FixedLagSmoother::FixedLagSmoother( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: fuse_optimizers::Optimizer(interfaces, std::move(graph)), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) +FixedLagSmoother::FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + , ignited_(false) + , optimization_running_(true) + , started_(false) + , optimization_request_(false) { params_.loadFromROS(interfaces_); @@ -97,30 +94,16 @@ FixedLagSmoother::FixedLagSmoother( optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&FixedLagSmoother::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + optimize_timer_ = rclcpp::create_timer(interfaces_, clock_, params_.optimization_period, + std::bind(&FixedLagSmoother::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group()); // Advertise a service that resets the optimizer to its initial state reset_service_server_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.reset_service), - std::bind( - &FixedLagSmoother::resetServiceCallback, - this, - std::placeholders::_1, - std::placeholders::_2 - ), - rclcpp::ServicesQoS(), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), + std::bind(&FixedLagSmoother::resetServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), interfaces_.get_node_base_interface()->get_default_callback_group()); } FixedLagSmoother::~FixedLagSmoother() @@ -129,27 +112,25 @@ FixedLagSmoother::~FixedLagSmoother() optimization_running_ = false; optimization_requested_.notify_all(); // Wait for the threads to shutdown - if (optimization_thread_.joinable()) { + if (optimization_thread_.joinable()) + { optimization_thread_.join(); } } void FixedLagSmoother::autostart() { - if (std::none_of( - sensor_models_.begin(), sensor_models_.end(), - [](const auto & element) {return element.second.ignition;})) // NOLINT(whitespace/braces) + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; setStartTime(rclcpp::Time(0, 1, RCL_ROS_TIME)); // Initialized - RCLCPP_INFO_STREAM( - logger_, - "No ignition sensors were specified. Optimization will begin immediately."); + RCLCPP_INFO_STREAM(logger_, "No ignition sensors were specified. Optimization will begin immediately."); } } -void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction & new_transaction) +void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) { timestamp_tracking_.addNewTransaction(new_transaction); } @@ -160,7 +141,8 @@ rclcpp::Time FixedLagSmoother::computeLagExpirationTime() const auto start_time = getStartTime(); auto now = timestamp_tracking_.currentStamp(); - if (0u == now.nanoseconds()) { + if (0u == now.nanoseconds()) + { return start_time; } @@ -168,32 +150,26 @@ rclcpp::Time FixedLagSmoother::computeLagExpirationTime() const return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; } -std::vector FixedLagSmoother::computeVariablesToMarginalize( - const rclcpp::Time & lag_expiration) +std::vector FixedLagSmoother::computeVariablesToMarginalize(const rclcpp::Time& lag_expiration) { auto marginalize_variable_uuids = std::vector(); timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); return marginalize_variable_uuids; } -void FixedLagSmoother::postprocessMarginalization( - const fuse_core::Transaction & marginal_transaction) +void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) { timestamp_tracking_.addMarginalTransaction(marginal_transaction); } void FixedLagSmoother::optimizationLoop() { - auto exit_wait_condition = [this]() - { - return - this->optimization_request_ || - !this->optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid(); - }; + auto exit_wait_condition = [this]() { + return this->optimization_request_ || !this->optimization_running_ || + !interfaces_.get_node_base_interface()->get_context()->is_valid(); + }; // Optimize constraints until told to exit - while (interfaces_.get_node_base_interface()->get_context()->is_valid() && - optimization_running_) + while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) { // Wait for the next signal to start the next optimization cycle // NOTE(CH3): Uninitialized, but it's ok since it's meant to get overwritten. @@ -205,8 +181,7 @@ void FixedLagSmoother::optimizationLoop() optimization_deadline = optimization_deadline_; } // If a shutdown is requested, exit now. - if (!optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid()) + if (!optimization_running_ || !interfaces_.get_node_base_interface()->get_context()->is_valid()) { break; } @@ -224,7 +199,8 @@ void FixedLagSmoother::optimizationLoop() processQueue(*new_transaction, lag_expiration_); // Skip this optimization cycle if the transaction is empty because something failed while // processing the pending transactions queue. - if (new_transaction->empty()) { + if (new_transaction->empty()) + { continue; } // Prepare for selecting the marginal variables @@ -232,20 +208,21 @@ void FixedLagSmoother::optimizationLoop() // Combine the new transactions with any marginal transaction from the end of the last cycle new_transaction->merge(marginal_transaction_); // Update the graph - try { + try + { graph_->update(*new_transaction); - } catch (const std::exception & ex) { + } + catch (const std::exception& ex) + { std::ostringstream oss; oss << "Graph:\n"; graph_->print(oss); oss << "\nTransaction:\n"; new_transaction->print(oss); - RCLCPP_FATAL_STREAM( - logger_, - "Failed to update graph with transaction: " - << ex.what() << "\nLeaving optimization loop and requesting node shutdown...\n" - << oss.str()); + RCLCPP_FATAL_STREAM(logger_, "Failed to update graph with transaction: " + << ex.what() << "\nLeaving optimization loop and requesting node shutdown...\n" + << oss.str()); rclcpp::shutdown(); break; } @@ -258,12 +235,11 @@ void FixedLagSmoother::optimizationLoop() // Abort if optimization failed. Not converging is not a failure because the solution found is // usable. - if (!summary_.IsSolutionUsable()) { - RCLCPP_FATAL_STREAM( - logger_, - "Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp.nanoseconds() << - ". Leaving optimization loop and requesting node shutdown..."); + if (!summary_.IsSolutionUsable()) + { + RCLCPP_FATAL_STREAM(logger_, "Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp.nanoseconds() + << ". Leaving optimization loop and requesting node shutdown..."); RCLCPP_INFO(logger_, summary_.FullReport().c_str()); rclcpp::shutdown(); break; @@ -272,21 +248,17 @@ void FixedLagSmoother::optimizationLoop() // Compute a transaction that marginalizes out those variables. lag_expiration_ = computeLagExpirationTime(); marginal_transaction_ = fuse_constraints::marginalizeVariables( - interfaces_.get_node_base_interface()->get_name(), - computeVariablesToMarginalize(lag_expiration_), - *graph_); + interfaces_.get_node_base_interface()->get_name(), computeVariablesToMarginalize(lag_expiration_), *graph_); // Perform any post-marginal cleanup postprocessMarginalization(marginal_transaction_); // Note: The marginal transaction will not be applied until the next optimization iteration // Log a warning if the optimization took too long auto optimization_complete = clock_->now(); - if (optimization_complete > optimization_deadline) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, - *clock_, - 10.0 * 1000, - "Optimization exceeded the configured duration by " - << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); + if (optimization_complete > optimization_deadline) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); } } } @@ -295,7 +267,8 @@ void FixedLagSmoother::optimizationLoop() void FixedLagSmoother::optimizerTimerCallback() { // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) { + if (!started_) + { return; } // If there is some pending work, trigger the next optimization cycle. @@ -306,7 +279,8 @@ void FixedLagSmoother::optimizerTimerCallback() std::lock_guard lock(pending_transactions_mutex_); optimization_request = !pending_transactions_.empty(); } - if (optimization_request) { + if (optimization_request) + { { std::lock_guard lock(optimization_requested_mutex_); optimization_request_ = true; @@ -316,14 +290,13 @@ void FixedLagSmoother::optimizerTimerCallback() } } -void FixedLagSmoother::processQueue( - fuse_core::Transaction & transaction, - const rclcpp::Time & lag_expiration) +void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration) { // We need to get the pending transactions from the queue std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - if (pending_transactions_.empty()) { + if (pending_transactions_.empty()) + { return; } @@ -337,37 +310,40 @@ void FixedLagSmoother::processQueue( // been optimized yet, and they will have to use a default zero state instead. This can easily // lead to local minima because the variables in the graph are not initialized properly, i.e. they // do not take the ignition sensor transaction into account. - if (ignited_) { + if (ignited_) + { // The ignition sensor transaction is assumed to be at the end of the queue, because it must be // the oldest one. If there is more than one ignition sensor transaction in the queue, it is // always the oldest one that started things up. ignited_ = false; const auto transaction_rbegin = pending_transactions_.rbegin(); - auto & element = *transaction_rbegin; - if (!sensor_models_.at(element.sensor_name).ignition) { + auto& element = *transaction_rbegin; + if (!sensor_models_.at(element.sensor_name).ignition) + { // We just started, but the oldest transaction is not from an ignition sensor. We will still // process the transaction, but we do not enforce it is processed individually. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " is not an ignition sensor transaction. " - << "This transaction will not be processed individually."); - } else { - if (applyMotionModels(element.sensor_name, *element.transaction)) { + RCLCPP_ERROR_STREAM(logger_, "The queued transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); + } + else + { + if (applyMotionModels(element.sensor_name, *element.transaction)) + { // Processing was successful. Add the results to the final transaction, delete this one, and // return, so the transaction from the ignition sensor is processed individually. transaction.merge(*element.transaction, true); erase(pending_transactions_, transaction_rbegin); - } else { + } + else + { // The motion model processing failed. When this happens to an ignition sensor transaction // there is no point on trying again next time, so we ignore this transaction. - RCLCPP_ERROR_STREAM( - logger_, - "The queued ignition transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " could not be processed. Ignoring this ignition transaction."); + RCLCPP_ERROR_STREAM(logger_, "The queued ignition transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition transaction."); // Remove the ignition transaction that just failed and purge all transactions after it. But // if we find another ignition transaction, we schedule it to be processed in the next @@ -375,21 +351,22 @@ void FixedLagSmoother::processQueue( erase(pending_transactions_, transaction_rbegin); const auto pending_ignition_transaction_iter = - std::find_if( - pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto & element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) - if (pending_ignition_transaction_iter == pending_transactions_.rend()) { + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) + if (pending_ignition_transaction_iter == pending_transactions_.rend()) + { // There is no other ignition transaction pending. We simply roll back to not started // state and all other pending transactions will be handled later in the transaction // callback, as usual. started_ = false; - } else { + } + else + { // Erase all transactions before the other ignition transaction pending. This other // ignition transaction will be processed in the next optimization cycle. - pending_transactions_.erase( - pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); + pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); ignited_ = true; } } @@ -406,47 +383,52 @@ void FixedLagSmoother::processQueue( // Attempt to process each pending transaction auto sensor_blacklist = std::vector(); auto transaction_riter = pending_transactions_.rbegin(); - while (transaction_riter != pending_transactions_.rend()) { - auto & element = *transaction_riter; - const auto & min_stamp = element.minStamp(); - if (min_stamp < lag_expiration) { - RCLCPP_DEBUG_STREAM( - logger_, - "The current lag expiration time is " - << lag_expiration.nanoseconds() << ". The queued transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " has a minimum involved timestamp of " << min_stamp.nanoseconds() << ", which is " - << (lag_expiration - min_stamp).nanoseconds() - << " seconds too old. Ignoring this transaction."); + while (transaction_riter != pending_transactions_.rend()) + { + auto& element = *transaction_riter; + const auto& min_stamp = element.minStamp(); + if (min_stamp < lag_expiration) + { + RCLCPP_DEBUG_STREAM(logger_, "The current lag expiration time is " + << lag_expiration.nanoseconds() << ". The queued transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " has a minimum involved timestamp of " << min_stamp.nanoseconds() + << ", which is " << (lag_expiration - min_stamp).nanoseconds() + << " seconds too old. Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); - } else if ( // NOLINT - std::find( - sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name - ) != sensor_blacklist.end()) + } + else if ( // NOLINT + std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) { // We should not process transactions from this sensor ++transaction_riter; - } else if (applyMotionModels(element.sensor_name, *element.transaction)) { + } + else if (applyMotionModels(element.sensor_name, *element.transaction)) + { // Processing was successful. Add the results to the final transaction, delete this one, and // move to the next. transaction.merge(*element.transaction, true); transaction_riter = erase(pending_transactions_, transaction_riter); - } else { + } + else + { // The motion model processing failed. // Check the transaction timeout to determine if it should be removed or skipped. - const auto & max_stamp = element.maxStamp(); - if (max_stamp + params_.transaction_timeout < current_time) { + const auto& max_stamp = element.maxStamp(); + if (max_stamp + params_.transaction_timeout < current_time) + { // Warn that this transaction has expired, then skip it. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.stamp().nanoseconds() << " and maximum involved stamp of " - << max_stamp.nanoseconds() << " from sensor " << element.sensor_name - << " could not be processed after " << (current_time - max_stamp).nanoseconds() - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); + RCLCPP_ERROR_STREAM(logger_, "The queued transaction with timestamp " + << element.stamp().nanoseconds() << " and maximum involved stamp of " + << max_stamp.nanoseconds() << " from sensor " << element.sensor_name + << " could not be processed after " << (current_time - max_stamp).nanoseconds() + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout.nanoseconds() + << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); - } else { + } + else + { // The motion model failed. Stop further processing of this sensor and try again next time. sensor_blacklist.push_back(element.sensor_name); ++transaction_riter; @@ -455,10 +437,8 @@ void FixedLagSmoother::processQueue( } } -bool FixedLagSmoother::resetServiceCallback( - const std::shared_ptr, - std::shared_ptr -) +bool FixedLagSmoother::resetServiceCallback(const std::shared_ptr, + std::shared_ptr) { // Tell all the plugins to stop stopPlugins(); @@ -494,20 +474,17 @@ bool FixedLagSmoother::resetServiceCallback( return true; } -void FixedLagSmoother::transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); - if (started_ && max_time < start_time) { - RCLCPP_DEBUG_STREAM( - logger_, - "Received a transaction before the start time from sensor '" - << sensor_name << "'.\n start_time: " << start_time.nanoseconds() - << ", maximum involved stamp: " << max_time.nanoseconds() << ", difference: " - << (start_time - max_time).nanoseconds() << "ns"); + if (started_ && max_time < start_time) + { + RCLCPP_DEBUG_STREAM(logger_, "Received a transaction before the start time from sensor '" + << sensor_name << "'.\n start_time: " << start_time.nanoseconds() + << ", maximum involved stamp: " << max_time.nanoseconds() + << ", difference: " << (start_time - max_time).nanoseconds() << "ns"); return; } { @@ -517,21 +494,19 @@ void FixedLagSmoother::transactionCallback( // Add the new transaction to the pending set // The pending set is arranged "smallest stamp last" to making popping off the back more // efficient - auto comparator = [](const rclcpp::Time & value, const TransactionQueueElement & element) - { - return value >= element.stamp(); - }; - auto position = std::upper_bound( - pending_transactions_.begin(), - pending_transactions_.end(), - transaction->stamp(), - comparator); - position = pending_transactions_.insert(position, {sensor_name, std::move(transaction)}); // NOLINT + auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { + return value >= element.stamp(); + }; + auto position = + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT // If we haven't "started" yet.. - if (!started_) { + if (!started_) + { // ...check if we should - if (sensor_models_.at(sensor_name).ignition) { + if (sensor_models_.at(sensor_name).ignition) + { started_ = true; ignited_ = true; start_time = position->minStamp(); @@ -545,31 +520,29 @@ void FixedLagSmoother::transactionCallback( // can use std::as_const: // https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( - std::remove_if( - pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - & min_time = start_time](const auto & transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); - } else { + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); + } + else + { // And purge out old transactions to limit the pending size while waiting for an ignition // sensor auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME); // NOTE(CH3): Uninitialized auto last_pending_time = pending_transactions_.front().stamp(); // rclcpp::Time doesn't allow negatives - if (rclcpp::Time( - params_.transaction_timeout.nanoseconds(), - last_pending_time.get_clock_type()) < - last_pending_time) + if (rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < + last_pending_time) { purge_time = last_pending_time - params_.transaction_timeout; } - while (!pending_transactions_.empty() && - pending_transactions_.back().maxStamp() < purge_time) + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) { pending_transactions_.pop_back(); } @@ -584,9 +557,7 @@ void FixedLagSmoother::transactionCallback( * @param[in] level The diagnostic status level * @param[in] message The diagnostic status message */ -diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus( - const int8_t level, - const std::string & message) +diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) { diagnostic_msgs::msg::DiagnosticStatus status; @@ -609,27 +580,21 @@ diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus( * @return The diagnostic status with the level and message corresponding to the optimization * termination type */ -diagnostic_msgs::msg::DiagnosticStatus terminationTypeToDiagnosticStatus( - const ceres::TerminationType termination_type) +diagnostic_msgs::msg::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) { - switch (termination_type) { + switch (termination_type) + { case ceres::TerminationType::CONVERGENCE: case ceres::TerminationType::USER_SUCCESS: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::OK, - "Optimization converged"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimization converged"); case ceres::TerminationType::NO_CONVERGENCE: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "Optimization didn't converge"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Optimization didn't converge"); default: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, - "Optimization failed"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Optimization failed"); } } -void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { Optimizer::setDiagnostics(status); @@ -642,24 +607,27 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe status.add("Pending Transactions", pending_transactions_.size()); } - if (started) { + if (started) + { // Add some optimization summary report fields to the diagnostics status if the optimizer has // started auto summary = decltype(summary_)(); { const std::unique_lock lock(optimization_mutex_, std::try_to_lock); - if (lock) { + if (lock) + { summary = summary_; - } else { + } + else + { status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimization running"); } } // summary.total_time_in_seconds is -1 if default-constructed - if (summary.total_time_in_seconds >= 0.0) { - status.add( - "Optimization Termination Type", - ceres::TerminationTypeToString(summary.termination_type)); + if (summary.total_time_in_seconds >= 0.0) + { + status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); status.add("Optimization Total Time [s]", summary.total_time_in_seconds); status.add("Optimization Iterations", summary.iterations.size()); status.add("Initial Cost", summary.initial_cost); @@ -673,18 +641,17 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe auto optimization_deadline = decltype(optimization_deadline_)(); { const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); - if (lock) { + if (lock) + { optimization_deadline = optimization_deadline_; } } - if (0u != optimization_deadline.nanoseconds()) { + if (0u != optimization_deadline.nanoseconds()) + { const auto optimization_request_time = optimization_deadline - params_.optimization_period; - const auto time_since_last_optimization_request = - clock_->now() - optimization_request_time; - status.add( - "Time Since Last Optimization Request [s]", - time_since_last_optimization_request.seconds()); + const auto time_since_last_optimization_request = clock_->now() - optimization_request_time; + status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.seconds()); } } } diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 7be160efa..02a6d02d7 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -37,7 +37,7 @@ #include #include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("fixed_lag_smoother_node"); diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index e23bd3353..5d9571eb3 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -52,30 +52,22 @@ namespace fuse_optimizers { -Optimizer::Optimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: interfaces_(interfaces), - clock_(interfaces.get_node_clock_interface()->get_clock()), - logger_(interfaces.get_node_logging_interface()->get_logger()), - graph_(std::move(graph)), - motion_model_loader_("fuse_core", "fuse_core::MotionModel"), - publisher_loader_("fuse_core", "fuse_core::Publisher"), - sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_( - interfaces.get_node_base_interface(), - interfaces.get_node_clock_interface(), - interfaces.get_node_logging_interface(), - interfaces.get_node_parameters_interface(), - interfaces.get_node_timers_interface(), - interfaces.get_node_topics_interface() - ), - callback_queue_(std::make_shared( - interfaces_.get_node_base_interface() - ->get_context())) +Optimizer::Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : interfaces_(interfaces) + , clock_(interfaces.get_node_clock_interface()->get_clock()) + , logger_(interfaces.get_node_logging_interface()->get_logger()) + , graph_(std::move(graph)) + , motion_model_loader_("fuse_core", "fuse_core::MotionModel") + , publisher_loader_("fuse_core", "fuse_core::Publisher") + , sensor_model_loader_("fuse_core", "fuse_core::SensorModel") + , diagnostic_updater_(interfaces.get_node_base_interface(), interfaces.get_node_clock_interface(), + interfaces.get_node_logging_interface(), interfaces.get_node_parameters_interface(), + interfaces.get_node_timers_interface(), interfaces.get_node_topics_interface()) + , callback_queue_(std::make_shared(interfaces_.get_node_base_interface()->get_context())) { - if (!graph) { + if (!graph) + { fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(interfaces_); graph_ = std::move(fuse_graphs::HashGraph::make_unique(hash_graph_params)); @@ -83,11 +75,9 @@ Optimizer::Optimizer( // add a ros1 style callback queue so that transactions can be processed in the optimiser's // executor - interfaces_.get_node_waitables_interface()->add_waitable( - callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); + interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); - diagnostic_updater_.add( - interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); + diagnostic_updater_.add(interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); diagnostic_updater_.setHardwareID("fuse"); // Wait for a valid time before loading any of the plugins @@ -122,48 +112,44 @@ void Optimizer::loadMotionModels() std::vector motion_model_config; std::unordered_set motion_model_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "motion_models."); + fuse_core::list_parameter_override_prefixes(interfaces_, "motion_models."); // declare config parameters for each model - for (const auto & param_name : motion_model_names) { - ModelConfig & config = motion_model_config.emplace_back(); + for (const auto& param_name : motion_model_names) + { + ModelConfig& config = motion_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.param_name = param_name + ".type"; - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this motion_model (eg: 'fuse_models::Unicycle2D')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this motion_model (eg: 'fuse_models::Unicycle2D')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the motion model rclcpp::Parameter motion_model_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter - if (motion_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (motion_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = motion_model_type_param.as_string(); } // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.param_name << - "' should be the string of a motion_model type " << - "for the motion_model named '" << config.name << - "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a motion_model type " + << "for the motion_model named '" << config.name << "'."); } } // now load the models defined above - for (const ModelConfig & config : motion_model_config) { + for (const ModelConfig& config : motion_model_config) + { // Create a motion_model object using pluginlib. This will throw if the plugin name is not // found. auto motion_model = motion_model_loader_.createUniqueInstance(config.type); @@ -194,120 +180,110 @@ void Optimizer::loadSensorModels() std::vector sensor_model_config; std::unordered_set sensor_model_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "sensor_models."); + fuse_core::list_parameter_override_prefixes(interfaces_, "sensor_models."); // declare config parameters for each model - for (const auto & param_name : sensor_model_names) { - ModelConfig & config = sensor_model_config.emplace_back(); + for (const auto& param_name : sensor_model_names) + { + ModelConfig& config = sensor_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.type_param_name = param_name + ".type"; config.models_param_name = param_name + ".motion_models"; config.ignition_param_name = param_name + ".ignition"; - // get the type parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.type_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.type_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this sensor_model " - "(eg: 'fuse_models::Acceleration2D')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.type_param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this sensor_model " + "(eg: 'fuse_models::Acceleration2D')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.type_param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the sensor model rclcpp::Parameter sensor_model_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.type_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.type_param_name); // extract the type string from the parameter - if (sensor_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (sensor_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = sensor_model_type_param.as_string(); } - // get the type parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.models_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.models_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; descr.description = "the list of motion models this sensor is associated with"; interfaces_.get_node_parameters_interface()->declare_parameter( - config.models_param_name, - rclcpp::ParameterValue(std::vector()), - descr - ); + config.models_param_name, rclcpp::ParameterValue(std::vector()), descr); } // get the model_list parameter for the sensor model rclcpp::Parameter sensor_model_model_list_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.models_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.models_param_name); // extract the model_list string from the parameter - if (sensor_model_model_list_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { + if (sensor_model_model_list_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { config.associated_motion_models = sensor_model_model_list_param.as_string_array(); } - // get the ignition parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.ignition_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.ignition_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; descr.description = "does the first message for this sensor start the optimizer"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.ignition_param_name, - rclcpp::ParameterValue(false), - descr - ); + interfaces_.get_node_parameters_interface()->declare_parameter(config.ignition_param_name, + rclcpp::ParameterValue(false), descr); } // get the model list parameter for the sensor model rclcpp::Parameter sensor_model_ignition_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.ignition_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.ignition_param_name); // extract the ignition bool from the parameter - if (sensor_model_ignition_param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) { + if (sensor_model_ignition_param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) + { config.ignition = sensor_model_ignition_param.as_bool(); } - // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.type_param_name << "' should be the string of a sensor_model type " - << "for the sensor_model named '" << config.name << "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.type_param_name + << "' should be the string of a sensor_model type " + << "for the sensor_model named '" << config.name << "'."); } } // now load the models defined above - for (const ModelConfig & config : sensor_model_config) { + for (const ModelConfig& config : sensor_model_config) + { // Create a sensor object using pluginlib. This will throw if the plugin name is not found. auto sensor_model = sensor_model_loader_.createUniqueInstance(config.type); // Initialize the sensor - sensor_model->initialize( - interfaces_, config.name, - std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); + sensor_model->initialize(interfaces_, config.name, + std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); // Store the sensor in a member variable for use later - sensor_models_.emplace( - config.name, - SensorModelInfo{std::move(sensor_model), config.ignition}); // NOLINT(whitespace/braces) + sensor_models_.emplace(config.name, + SensorModelInfo{ std::move(sensor_model), config.ignition }); // NOLINT(whitespace/braces) // Parse out the list of associated motion models, if any associated_motion_models_[config.name] = config.associated_motion_models; - for (const auto & motion_model_name : config.associated_motion_models) { - if (motion_models_.find(motion_model_name) == motion_models_.end()) { - RCLCPP_WARN_STREAM( - logger_, - "Sensor model '" - << config.name << "' is configured to use motion model '" << motion_model_name - << "', but no motion model with that name currently exists. " - << "This is likely a configuration error."); + for (const auto& motion_model_name : config.associated_motion_models) + { + if (motion_models_.find(motion_model_name) == motion_models_.end()) + { + RCLCPP_WARN_STREAM(logger_, "Sensor model '" << config.name << "' is configured to use motion model '" + << motion_model_name + << "', but no motion model with that name currently exists. " + << "This is likely a configuration error."); } } } diagnostic_updater_.force_update(); } - void Optimizer::loadPublishers() { // struct for readability @@ -322,49 +298,45 @@ void Optimizer::loadPublishers() std::vector publisher_config; std::unordered_set publisher_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "publishers."); + fuse_core::list_parameter_override_prefixes(interfaces_, "publishers."); // declare config parameters for each model - for (const auto & param_name : publisher_names) { - PublisherConfig & config = publisher_config.emplace_back(); + for (const auto& param_name : publisher_names) + { + PublisherConfig& config = publisher_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.param_name = param_name + ".type"; - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this publisher " - "(eg: 'fuse_publishers::Path2DPublisher')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this publisher " + "(eg: 'fuse_publishers::Path2DPublisher')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the publisher rclcpp::Parameter publisher_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter - if (publisher_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (publisher_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = publisher_type_param.as_string(); } // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.param_name << - "' should be the string of a publisher type " << - "for the publisher named '" << config.name << - "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a publisher type " + << "for the publisher named '" << config.name << "'."); } } // now load the models defined above - for (const PublisherConfig & config : publisher_config) { + for (const PublisherConfig& config : publisher_config) + { // Create a publisher object using pluginlib. This will throw if the plugin name is not found. auto publisher = publisher_loader_.createUniqueInstance(config.type); // Initialize the publisher @@ -376,82 +348,85 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -bool Optimizer::applyMotionModels( - const std::string & sensor_name, - fuse_core::Transaction & transaction) const +bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const { // Check for trivial cases where we don't have to do anything auto iter = associated_motion_models_.find(sensor_name); - if (iter == associated_motion_models_.end()) { + if (iter == associated_motion_models_.end()) + { return true; } // Generate constraints for each configured motion model - const auto & motion_model_names = iter->second; + const auto& motion_model_names = iter->second; bool success = true; - for (const auto & motion_model_name : motion_model_names) { - try { + for (const auto& motion_model_name : motion_model_names) + { + try + { success &= motion_models_.at(motion_model_name)->apply(transaction); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Error generating constraints for sensor '" << sensor_name << "' from motion model '" - << motion_model_name << "'. Error: " << - e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Error generating constraints for sensor '" << sensor_name << "' from motion model '" + << motion_model_name + << "'. Error: " << e.what()); success = false; } } return success; } -void Optimizer::notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) { - for (const auto & name__sensor_model : sensor_models_) { - try { + for (const auto& name__sensor_model : sensor_models_) + { + try + { name__sensor_model.second.model->graphCallback(graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling graphCallback() on sensor '" << name__sensor_model.first - << "'. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on sensor '" << name__sensor_model.first + << "'. Error: " << e.what()); continue; } } - for (const auto & name__motion_model : motion_models_) { - try { + for (const auto& name__motion_model : motion_models_) + { + try + { name__motion_model.second->graphCallback(graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling graphCallback() on motion model '" << name__motion_model.first - << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on motion model '" << name__motion_model.first + << ". Error: " << e.what()); continue; } } - for (const auto & name__publisher : publishers_) { - try { + for (const auto& name__publisher : publishers_) + { + try + { name__publisher.second->notify(transaction, graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling notify() on publisher '" << name__publisher.first - << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, + "Failed calling notify() on publisher '" << name__publisher.first << ". Error: " << e.what()); continue; } } } -void Optimizer::injectCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void Optimizer::injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // We are going to insert a call to the derived class's transactionCallback() method into the // global callback queue. This returns execution to the sensor's thread quickly by moving the // transaction processing to the optimizer's thread. And by using the existing ROS callback queue, // we simplify the threading model of the optimizer. auto callback = std::make_shared>( - std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))); + std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))); callback_queue_->addCallback(callback); } @@ -462,13 +437,16 @@ void Optimizer::clearCallbacks() void Optimizer::startPlugins() { - for (const auto & name_plugin : motion_models_) { + for (const auto& name_plugin : motion_models_) + { name_plugin.second->start(); } - for (const auto & name_plugin : sensor_models_) { + for (const auto& name_plugin : sensor_models_) + { name_plugin.second.model->start(); } - for (const auto & name_plugin : publishers_) { + for (const auto& name_plugin : publishers_) + { name_plugin.second->start(); } @@ -477,22 +455,26 @@ void Optimizer::startPlugins() void Optimizer::stopPlugins() { - for (const auto & name_plugin : publishers_) { + for (const auto& name_plugin : publishers_) + { name_plugin.second->stop(); } - for (const auto & name_plugin : sensor_models_) { + for (const auto& name_plugin : sensor_models_) + { name_plugin.second.model->stop(); } - for (const auto & name_plugin : motion_models_) { + for (const auto& name_plugin : motion_models_) + { name_plugin.second->stop(); } diagnostic_updater_.force_update(); } -void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { - if (!clock_->started()) { + if (!clock_->started()) + { status.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for valid ROS time"); return; } @@ -501,20 +483,11 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists"); - auto print_key = [](const std::string & result, const auto & entry) - { - return result + entry.first + ' '; - }; - - status.add( - "Sensor Models", - std::accumulate(sensor_models_.begin(), sensor_models_.end(), std::string(), print_key)); - status.add( - "Motion Models", - std::accumulate(motion_models_.begin(), motion_models_.end(), std::string(), print_key)); - status.add( - "Publishers", - std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); + auto print_key = [](const std::string& result, const auto& entry) { return result + entry.first + ' '; }; + + status.add("Sensor Models", std::accumulate(sensor_models_.begin(), sensor_models_.end(), std::string(), print_key)); + status.add("Motion Models", std::accumulate(motion_models_.begin(), motion_models_.end(), std::string(), print_key)); + status.add("Publishers", std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index 60038053d..a2fbd3a04 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -46,19 +46,21 @@ namespace fuse_optimizers { rclcpp::Time VariableStampIndex::currentStamp() const { - auto compare_stamps = [](const StampedMap::value_type & lhs, const StampedMap::value_type & rhs) - { - return lhs.second < rhs.second; - }; + auto compare_stamps = [](const StampedMap::value_type& lhs, const StampedMap::value_type& rhs) { + return lhs.second < rhs.second; + }; auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); - if (iter != stamped_index_.end()) { + if (iter != stamped_index_.end()) + { return iter->second; - } else { + } + else + { return rclcpp::Time(0, 0, RCL_ROS_TIME); } } -void VariableStampIndex::addNewTransaction(const fuse_core::Transaction & transaction) +void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) { applyAddedVariables(transaction); applyAddedConstraints(transaction); @@ -66,7 +68,7 @@ void VariableStampIndex::addNewTransaction(const fuse_core::Transaction & transa applyRemovedVariables(transaction); } -void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction & transaction) +void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction& transaction) { // Only the removed variables and removed constraints should be applied to the VariableStampIndex // No variables will be added by a marginal transaction, and the added constraints add variable @@ -76,42 +78,47 @@ void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction & t applyRemovedVariables(transaction); } -void VariableStampIndex::applyAddedConstraints(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyAddedConstraints(const fuse_core::Transaction& transaction) { - for (const auto & constraint : transaction.addedConstraints()) { - constraints_[constraint.uuid()].insert( - constraint.variables().begin(), - constraint.variables().end()); - for (const auto & variable_uuid : constraint.variables()) { + for (const auto& constraint : transaction.addedConstraints()) + { + constraints_[constraint.uuid()].insert(constraint.variables().begin(), constraint.variables().end()); + for (const auto& variable_uuid : constraint.variables()) + { variables_[variable_uuid].insert(constraint.uuid()); } } } -void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& transaction) { - for (const auto & variable : transaction.addedVariables()) { - auto stamped_variable = dynamic_cast(&variable); - if (stamped_variable) { + for (const auto& variable : transaction.addedVariables()) + { + auto stamped_variable = dynamic_cast(&variable); + if (stamped_variable) + { stamped_index_[variable.uuid()] = stamped_variable->stamp(); } variables_[variable.uuid()]; // Add an empty set of constraints } } -void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction& transaction) { - for (const auto & constraint_uuid : transaction.removedConstraints()) { - for (auto & variable_uuid : constraints_[constraint_uuid]) { + for (const auto& constraint_uuid : transaction.removedConstraints()) + { + for (auto& variable_uuid : constraints_[constraint_uuid]) + { variables_[variable_uuid].erase(constraint_uuid); } constraints_.erase(constraint_uuid); } } -void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& transaction) { - for (const auto & variable_uuid : transaction.removedVariables()) { + for (const auto& variable_uuid : transaction.removedVariables()) + { stamped_index_.erase(variable_uuid); variables_.erase(variable_uuid); } diff --git a/fuse_optimizers/test/launch_tests/common.hpp b/fuse_optimizers/test/launch_tests/common.hpp index 43d1ca0a2..3beac67fc 100644 --- a/fuse_optimizers/test/launch_tests/common.hpp +++ b/fuse_optimizers/test/launch_tests/common.hpp @@ -42,7 +42,6 @@ #include #include - /** * @brief Helper function to print the elements of std::vector objects * @@ -50,12 +49,13 @@ * @param[in] v A vector to print into the stream * @return The output stream with the vector printed into it */ -template -std::ostream & operator<<(std::ostream & os, const std::vector & v) +template +std::ostream& operator<<(std::ostream& os, const std::vector& v) { os << '['; - if (!v.empty()) { + if (!v.empty()) + { std::copy(v.begin(), v.end() - 1, std::ostream_iterator(os, ", ")); os << v.back(); } @@ -72,12 +72,13 @@ std::ostream & operator<<(std::ostream & os, const std::vector & v) * @param[in] m An unordered map with the keys to print into the stream * @return The output stream with the vector printed into it */ -template -std::ostream & operator<<(std::ostream & os, const std::unordered_map & m) +template +std::ostream& operator<<(std::ostream& os, const std::unordered_map& m) { os << '['; - for (const auto & entry : m) { + for (const auto& entry : m) + { os << entry.first << ", "; } @@ -94,28 +95,23 @@ std::ostream & operator<<(std::ostream & os, const std::unordered_map & m) * @param[in] rhs An unordered map of key strings * @return A vector with the symmetric difference strings */ -template -std::vector set_symmetric_difference( - const std::vector & lhs, - const std::unordered_map & rhs) +template +std::vector set_symmetric_difference(const std::vector& lhs, + const std::unordered_map& rhs) { // Retrieve the keys: std::vector rhs_keys; - std::transform( - rhs.begin(), rhs.end(), std::back_inserter(rhs_keys), - [](const auto & pair) {return pair.first;}); // NOLINT(whitespace/braces) + std::transform(rhs.begin(), rhs.end(), std::back_inserter(rhs_keys), + [](const auto& pair) { return pair.first; }); // NOLINT(whitespace/braces) // Sort the keys so we can use std::set_symmetric_difference: std::sort(rhs_keys.begin(), rhs_keys.end()); // Compute the symmetric difference: std::vector diff; - std::set_symmetric_difference( - lhs.begin(), lhs.end(), rhs_keys.begin(), - rhs_keys.end(), std::back_inserter(diff)); + std::set_symmetric_difference(lhs.begin(), lhs.end(), rhs_keys.begin(), rhs_keys.end(), std::back_inserter(diff)); return diff; } - #endif // FUSE_OPTIMIZERS__TEST_COMMON_HPP_ // NOLINT{build/header_guard} diff --git a/fuse_optimizers/test/launch_tests/example_optimizer.hpp b/fuse_optimizers/test/launch_tests/example_optimizer.hpp index 5b6f5cc98..1aed0d680 100644 --- a/fuse_optimizers/test/launch_tests/example_optimizer.hpp +++ b/fuse_optimizers/test/launch_tests/example_optimizer.hpp @@ -40,7 +40,6 @@ #include - /** * @brief Example optimizer that exposes the motion and sensor models, and the publishers, so we can * check the expected ones are loaded. @@ -50,32 +49,28 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer public: FUSE_SMART_PTR_DEFINITIONS(ExampleOptimizer) - ExampleOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ) - : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + ExampleOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) { } - const MotionModels & getMotionModels() const + const MotionModels& getMotionModels() const { return motion_models_; } - const SensorModels & getSensorModels() const + const SensorModels& getSensorModels() const { return sensor_models_; } - const Publishers & getPublishers() const + const Publishers& getPublishers() const { return publishers_; } - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override { (void)sensor_name; (void)transaction; diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp index eafd79d6e..aff3502e1 100644 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp @@ -54,16 +54,14 @@ class FixedLagIgnitionFixture : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -81,7 +79,7 @@ class FixedLagIgnitionFixture : public ::testing::Test return received_odom_msg_; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; nav_msgs::msg::Odometry::SharedPtr received_odom_msg_; std::mutex received_odom_mutex_; @@ -93,12 +91,10 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) executor_->add_node(node); auto relative_pose_publisher = - node->create_publisher( - "/relative_pose", 5); + node->create_publisher("/relative_pose", 5); - auto odom_subscriber = - node->create_subscription( - "/odom", 5, std::bind(&FixedLagIgnitionFixture::odom_callback, this, std::placeholders::_1)); + auto odom_subscriber = node->create_subscription( + "/odom", 5, std::bind(&FixedLagIgnitionFixture::odom_callback, this, std::placeholders::_1)); // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify. ASSERT_TRUE(node->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); @@ -130,8 +126,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) // The 'set_pose' service call triggers all of the sensors to resubscribe to their topics. // I need to wait for those subscribers to be ready before sending them sensor data. rclcpp::Time subscriber_timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((relative_pose_publisher->get_subscription_count() < 1u) && - (node->now() < subscriber_timeout)) + while ((relative_pose_publisher->get_subscription_count() < 1u) && (node->now() < subscriber_timeout)) { rclcpp::sleep_for(std::chrono::milliseconds(10)); } @@ -175,8 +170,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) // Wait for the optimizer to process all queued transactions and publish the last odometry msg rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(1.0); auto odom_msg = nav_msgs::msg::Odometry::SharedPtr(); - while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, - RCL_ROS_TIME)) && (node->now() < result_timeout)) + while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, RCL_ROS_TIME)) && (node->now() < result_timeout)) { rclcpp::sleep_for(std::chrono::milliseconds(100)); odom_msg = this->get_last_odom_msg(); @@ -194,7 +188,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) } // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_optimizers/test/launch_tests/test_optimizer.cpp b/fuse_optimizers/test/launch_tests/test_optimizer.cpp index 1e765ea19..3356c9b0c 100644 --- a/fuse_optimizers/test/launch_tests/test_optimizer.cpp +++ b/fuse_optimizers/test/launch_tests/test_optimizer.cpp @@ -42,7 +42,6 @@ #include // NOLINT - TEST(Optimizer, Constructor) { // Create optimizer: @@ -50,54 +49,47 @@ TEST(Optimizer, Constructor) ExampleOptimizer optimizer(*node); // Check the motion and sensor models, and publishers were loaded: - const auto & motion_models = optimizer.getMotionModels(); - const auto & sensor_models = optimizer.getSensorModels(); - const auto & publishers = optimizer.getPublishers(); + const auto& motion_models = optimizer.getMotionModels(); + const auto& sensor_models = optimizer.getSensorModels(); + const auto& publishers = optimizer.getPublishers(); EXPECT_FALSE(motion_models.empty()); EXPECT_FALSE(sensor_models.empty()); EXPECT_FALSE(publishers.empty()); // Check the expected motion and sensor models, and publisher were loaded: - const std::vector expected_motion_models = {"noisy_unicycle_2d", "unicycle_2d"}; - const std::vector expected_sensor_models = {"imu", "laser_localization", // NOLINT - "unicycle_2d_ignition", "wheel_odometry"}; - const std::vector expected_publishers = - {"odometry_publisher", "serialized_publisher"}; + const std::vector expected_motion_models = { "noisy_unicycle_2d", "unicycle_2d" }; + const std::vector expected_sensor_models = { "imu", "laser_localization", // NOLINT + "unicycle_2d_ignition", "wheel_odometry" }; + const std::vector expected_publishers = { "odometry_publisher", "serialized_publisher" }; ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) - << expected_motion_models << " is not sorted."; + << expected_motion_models << " is not sorted."; ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) - << expected_sensor_models << " is not sorted."; + << expected_sensor_models << " is not sorted."; ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) - << expected_publishers << " is not sorted."; + << expected_publishers << " is not sorted."; // Compute the symmetric difference between the expected and actual motion and sensor models, and // publishers: - const auto difference_motion_models = set_symmetric_difference( - expected_motion_models, - motion_models); - const auto difference_sensor_models = set_symmetric_difference( - expected_sensor_models, - sensor_models); + const auto difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); + const auto difference_sensor_models = set_symmetric_difference(expected_sensor_models, sensor_models); const auto difference_publishers = set_symmetric_difference(expected_publishers, publishers); // Check the symmetric difference is empty, i.e. the actual motion and sensor models, and // publishers are the same as the expected ones: EXPECT_TRUE(difference_motion_models.empty()) - << "Actual: " << motion_models << "\nExpected: " << expected_motion_models - << "\nDifference: " << difference_motion_models; + << "Actual: " << motion_models << "\nExpected: " << expected_motion_models + << "\nDifference: " << difference_motion_models; EXPECT_TRUE(difference_sensor_models.empty()) - << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models - << "\nDifference: " << difference_sensor_models; + << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models + << "\nDifference: " << difference_sensor_models; EXPECT_TRUE(difference_publishers.empty()) - << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << - difference_publishers; + << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << difference_publishers; } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index bcf9df18d..02c2a4506 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -59,10 +59,8 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp public: FUSE_VARIABLE_DEFINITIONS(StampedVariable) - explicit StampedVariable(const rclcpp::Time & stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) - : fuse_core::Variable(fuse_core::uuid::generate()), - fuse_variables::Stamped(stamp), - data_{} + explicit StampedVariable(const rclcpp::Time& stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::Variable(fuse_core::uuid::generate()), fuse_variables::Stamped(stamp), data_{} { } @@ -71,17 +69,17 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } @@ -98,12 +96,12 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -117,9 +115,7 @@ class UnstampedVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(UnstampedVariable) - UnstampedVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_{} + UnstampedVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} { } @@ -128,17 +124,17 @@ class UnstampedVariable : public fuse_core::Variable return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } @@ -155,11 +151,11 @@ class UnstampedVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -175,40 +171,32 @@ class GenericConstraint : public fuse_core::Constraint GenericConstraint() = default; - GenericConstraint( - const std::string & source, - std::initializer_list variable_uuids) - : Constraint(source, variable_uuids) + GenericConstraint(const std::string& source, std::initializer_list variable_uuids) + : Constraint(source, variable_uuids) { } - explicit GenericConstraint(const std::string & source, const fuse_core::UUID & variable1) - : fuse_core::Constraint(source, {variable1}) + explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) + : fuse_core::Constraint(source, { variable1 }) { } - GenericConstraint( - const std::string & source, - const fuse_core::UUID & variable1, - const fuse_core::UUID & variable2) - : fuse_core::Constraint(source, {variable1, variable2}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint(source, { variable1, variable2 }) { } - GenericConstraint( - const std::string & source, - const fuse_core::UUID & variable1, - const fuse_core::UUID & variable2, - const fuse_core::UUID & variable3) - : fuse_core::Constraint(source, {variable1, variable2, variable3}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2, + const fuse_core::UUID& variable3) + : fuse_core::Constraint(source, { variable1, variable2, variable3 }) { } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } - ceres::CostFunction * costFunction() const override + ceres::CostFunction* costFunction() const override { return nullptr; } @@ -224,16 +212,15 @@ class GenericConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; BOOST_CLASS_EXPORT(GenericConstraint); - TEST(VariableStampIndex, Size) { // Create an empty index @@ -322,7 +309,7 @@ TEST(VariableStampIndex, Query) index.query(rclcpp::Time(1, 500000, RCL_ROS_TIME), std::back_inserter(actual1)); EXPECT_EQ(expected1, actual1); - auto expected2 = std::vector{x1->uuid(), l1->uuid()}; + auto expected2 = std::vector{ x1->uuid(), l1->uuid() }; std::sort(expected2.begin(), expected2.end()); auto actual2 = std::vector(); index.query(rclcpp::Time(2, 500000, RCL_ROS_TIME), std::back_inserter(actual2)); @@ -375,7 +362,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(4u, index.size()); // And the marginal constraint x3->l1 should not affect future queries - auto expected = std::vector{l1->uuid()}; + auto expected = std::vector{ l1->uuid() }; std::sort(expected.begin(), expected.end()); auto actual = std::vector(); index.query(rclcpp::Time(2, 500000, RCL_ROS_TIME), std::back_inserter(actual)); diff --git a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp index 78e8a0a16..516391022 100644 --- a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp @@ -47,7 +47,6 @@ #include #include - namespace fuse_publishers { @@ -79,9 +78,8 @@ class Path2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -97,20 +95,17 @@ class Path2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ fuse_core::UUID device_id_; //!< The UUID of the device to be published - std::string frame_id_; //!< The name of the frame for this path + std::string frame_id_; //!< The name of the frame for this path //!< The publisher that sends the entire robot trajectory as a path rclcpp::Publisher::SharedPtr path_publisher_; diff --git a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp index b37ac61c1..a00b66688 100644 --- a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp @@ -61,7 +61,6 @@ #include - namespace fuse_publishers { @@ -134,9 +133,8 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -166,9 +164,8 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Timer-based callback that publishes the latest map->odom transform @@ -180,38 +177,32 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher void tfPublishTimerCallback(); protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - using Synchronizer = StampedVariableSynchronizer< - fuse_variables::Orientation2DStamped, - fuse_variables::Position2DStamped>; - - std::string base_frame_; //!< The name of the robot's base_link frame - fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + using Synchronizer = + StampedVariableSynchronizer; + + std::string base_frame_; //!< The name of the robot's base_link frame + fuse_core::UUID device_id_; //!< The UUID of the device to be published rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger - std::string map_frame_; //!< The name of the robot's map frame + std::string map_frame_; //!< The name of the robot's map frame std::string odom_frame_; //!< The name of the odom frame for this pose (or empty if the odom is //!< not used) rclcpp::Publisher::SharedPtr pose_publisher_; - rclcpp::Publisher< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_; - bool publish_to_tf_; //!< Flag indicating the pose should be sent to the tf system as well as the - //!< pose topics + rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; + bool publish_to_tf_; //!< Flag indicating the pose should be sent to the tf system as well as the + //!< pose topics Synchronizer::UniquePtr synchronizer_; //!< Object that tracks the latest common timestamp of //!< multiple variables - std::unique_ptr tf_buffer_; //!< TF2 object that supports querying transforms by - //!< time and frame id + std::unique_ptr tf_buffer_; //!< TF2 object that supports querying transforms by + //!< time and frame id std::unique_ptr tf_listener_; //!< TF2 object that subscribes to the //!< tf topics and inserts the received //!< transforms into the tf buffer @@ -219,11 +210,11 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher //!< Publish the map->odom or map->base transform to the tf system std::shared_ptr tf_publisher_ = nullptr; - rclcpp::TimerBase::SharedPtr tf_publish_timer_; //!< Timer that publishes tf messages to ensure - //!< the tf transform doesn't get stale - rclcpp::Duration tf_timeout_; //!< The max time to wait for a tf transform to become available + rclcpp::TimerBase::SharedPtr tf_publish_timer_; //!< Timer that publishes tf messages to ensure + //!< the tf transform doesn't get stale + rclcpp::Duration tf_timeout_; //!< The max time to wait for a tf transform to become available geometry_msgs::msg::TransformStamped tf_transform_; //!< The transform to be published to tf - bool use_tf_lookup_; //!< Internal flag indicating that a tf frame lookup is required + bool use_tf_lookup_; //!< Internal flag indicating that a tf frame lookup is required }; } // namespace fuse_publishers diff --git a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp index 764aac037..fc62ba01a 100644 --- a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_publishers { @@ -71,9 +70,8 @@ class SerializedPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -89,19 +87,15 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ /** * @brief Publish the serialized graph @@ -110,17 +104,14 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * whenever needed * @param[in] stamp A rclcpp::Time stamp used for the serialized graph message published */ - void graphPublisherCallback( - fuse_core::Graph::ConstSharedPtr graph, - const rclcpp::Time & stamp) const; + void graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const; std::string frame_id_; //!< The name of the frame for the serialized graph and transaction //!< messages published rclcpp::Publisher::SharedPtr graph_publisher_; rclcpp::Publisher::SharedPtr transaction_publisher_; - using GraphPublisherCallback = - std::function; + using GraphPublisherCallback = std::function; using GraphPublisherThrottledCallback = fuse_core::ThrottledCallback; GraphPublisherThrottledCallback graph_publisher_throttled_callback_; //!< The graph publisher //!< throttled callback diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h index e1c240cad..5f390ea09 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h @@ -35,8 +35,7 @@ #ifndef FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ #define FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ -#warning \ - This header is obsolete, please include fuse_publishers/stamped_variable_synchronizer.hpp instead +#warning This header is obsolete, please include fuse_publishers/stamped_variable_synchronizer.hpp instead #include diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp index 92cca72b1..0de70671a 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp @@ -45,7 +45,6 @@ #include - namespace fuse_publishers { @@ -65,7 +64,7 @@ namespace fuse_publishers * * This class only functions with variables derived from the fuse_variables::Stamped base class. */ -template +template class StampedVariableSynchronizer { public: @@ -76,7 +75,7 @@ class StampedVariableSynchronizer * * @param[in] device_id The device id to use for all variable types */ - explicit StampedVariableSynchronizer(const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit StampedVariableSynchronizer(const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Find the latest timestamp for which variables of all the specified template types exist @@ -87,12 +86,10 @@ class StampedVariableSynchronizer * @param[in] graph The complete graph * @return The latest timestamp shared by all requested variable types */ - rclcpp::Time findLatestCommonStamp( - const fuse_core::Transaction & transaction, - const fuse_core::Graph & graph); + rclcpp::Time findLatestCommonStamp(const fuse_core::Transaction& transaction, const fuse_core::Graph& graph); private: - fuse_core::UUID device_id_; //!< The device_id to use with the Stamped classes + fuse_core::UUID device_id_; //!< The device_id to use with the Stamped classes rclcpp::Time latest_common_stamp_; //!< The previously discovered common stamp /** @@ -103,8 +100,8 @@ class StampedVariableSynchronizer * @param[in] graph The complete graph, used to verify that all requested variables exist * for a given time */ - template - void updateTime(const VariableRange & variable_range, const fuse_core::Graph & graph); + template + void updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph); }; namespace detail @@ -113,79 +110,79 @@ namespace detail /** * @brief Some helper structs for testing properties of all types in a template parameter pack */ -template +template struct bool_pack; -template +template using all_true_helper = std::is_same, bool_pack>; /** * @brief Test if a property is true for all types in a template parameter pack. This is a type. */ -template +template using all_true = all_true_helper; /** * @brief Test if a property is true for all types in a template parameter pack. This is a boolean * value. */ -template +template constexpr bool allTrue = all_true::value; /** * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a type. */ -template +template using is_stamped = std::is_base_of; /** * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a boolean * value. */ -template +template constexpr bool isStamped = is_stamped::value; /** * @brief Test if a class is derived from the fuse_core::Variable base class. This is a type. */ -template +template using is_variable = std::is_base_of; /** * @brief Test if a class is derived from the fuse_core::Variable base class. This is a boolean * value. */ -template +template constexpr bool isVariable = is_variable::value; /** * @brief Test if a class is derived from both the fuse_core::Variable base class and the * fuse_variables::Stamped base class. This is a type. */ -template +template struct is_stamped_variable { - static constexpr bool value = isStamped&& isVariable; + static constexpr bool value = isStamped && isVariable; }; /** * @brief Test if a class is derived from both the fuse_core::Variable base class and the * fuse_variables::Stamped base class. This is a boolean value. */ -template +template constexpr bool isStampedVariable = is_stamped_variable::value; /** * @brief Test if all of the template parameter pack types are fuse_core::Variable and * fuse_variables::Stamped. This is a type. */ -template +template using all_stamped_variables = all_true...>; /** * @brief Test if all of the template parameter pack types are fuse_core::Variable and * fuse_variables::Stamped. This is a boolean value. */ -template +template constexpr bool allStampedVariables = all_stamped_variables::value; /** @@ -201,12 +198,11 @@ constexpr bool allStampedVariables = all_stamped_variables::value; * @param[in] device_id The device id used to construct all variable types * @return True if all variables exist, false otherwise */ -template +template struct all_variables_exist { - static bool value( - const fuse_core::Graph & /*graph*/, const rclcpp::Time & /*stamp*/, - const fuse_core::UUID & /*device_id*/) + static bool value(const fuse_core::Graph& /*graph*/, const rclcpp::Time& /*stamp*/, + const fuse_core::UUID& /*device_id*/) { return true; } @@ -223,12 +219,10 @@ struct all_variables_exist * @param[in] device_id The device id used to construct all variable types * @return True if all variables exist, false otherwise */ -template +template struct all_variables_exist { - static bool value( - const fuse_core::Graph & graph, const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) + static bool value(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id) { return graph.variableExists(T(stamp, device_id).uuid()) && all_variables_exist::value(graph, stamp, device_id); @@ -246,10 +240,10 @@ struct all_variables_exist * @param[in] variable The variable to check against the template parameter pack * @return True if the variable's type is part of the template parameter pack, false otherwise */ -template +template struct is_variable_in_pack { - static bool value(const fuse_core::Variable & /*variable*/) + static bool value(const fuse_core::Variable& /*variable*/) { return false; } @@ -264,65 +258,62 @@ struct is_variable_in_pack * @param[in] variable The variable to check against the template parameter pack * @return True if the variable's type is part of the template parameter pack, false otherwise */ -template +template struct is_variable_in_pack { - static bool value(const fuse_core::Variable & variable) + static bool value(const fuse_core::Variable& variable) { - auto derived = dynamic_cast(&variable); - return static_cast(derived) || - is_variable_in_pack::value(variable); + auto derived = dynamic_cast(&variable); + return static_cast(derived) || is_variable_in_pack::value(variable); } }; } // namespace detail -template -StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core::UUID & device_id) -: device_id_(device_id), +template +StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core::UUID& device_id) + : device_id_(device_id) + , // NOTE(CH3): Uninitialized, for getting latest We use RCL_ROS_TIME so time comparisons are // consistent latest_common_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { - static_assert( - detail::allStampedVariables, "All synchronized types must be derived from both " - "fuse_core::Variable and fuse_variable::Stamped."); + static_assert(detail::allStampedVariables, "All synchronized types must be derived from both " + "fuse_core::Variable and fuse_variable::Stamped."); static_assert(sizeof...(Ts) > 0, "At least one type must be specified."); } -template -rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp( - const fuse_core::Transaction & transaction, - const fuse_core::Graph & graph) +template +rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp(const fuse_core::Transaction& transaction, + const fuse_core::Graph& graph) { // Clear the previous stamp if the variable was deleted if (0u != latest_common_stamp_.nanoseconds() && - !detail::all_variables_exist::value(graph, latest_common_stamp_, device_id_)) + !detail::all_variables_exist::value(graph, latest_common_stamp_, device_id_)) { latest_common_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); } // Search the transaction for more recent variables updateTime(transaction.addedVariables(), graph); // If no common timestamp was found, search the whole graph for the most recent variable set - if (0u == latest_common_stamp_.nanoseconds()) { + if (0u == latest_common_stamp_.nanoseconds()) + { updateTime(graph.getVariables(), graph); } return latest_common_stamp_; } -template -template -void StampedVariableSynchronizer::updateTime( - const VariableRange & variable_range, - const fuse_core::Graph & graph) +template +template +void StampedVariableSynchronizer::updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph) { - for (const auto & candidate_variable : variable_range) { - if (detail::is_variable_in_pack::value(candidate_variable)) { - const auto & stamped_variable = - dynamic_cast(candidate_variable); - if ((stamped_variable.stamp() > latest_common_stamp_) && - (stamped_variable.deviceId() == device_id_) && - (detail::all_variables_exist::value(graph, stamped_variable.stamp(), device_id_))) + for (const auto& candidate_variable : variable_range) + { + if (detail::is_variable_in_pack::value(candidate_variable)) + { + const auto& stamped_variable = dynamic_cast(candidate_variable); + if ((stamped_variable.stamp() > latest_common_stamp_) && (stamped_variable.deviceId() == device_id_) && + (detail::all_variables_exist::value(graph, stamped_variable.stamp(), device_id_))) { latest_common_stamp_ = stamped_variable.stamp(); } diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 69c5be8f4..1b9675ae1 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -58,16 +58,12 @@ PLUGINLIB_EXPORT_CLASS(fuse_publishers::Path2DPublisher, fuse_core::Publisher); namespace fuse_publishers { -Path2DPublisher::Path2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - frame_id_("map") +Path2DPublisher::Path2DPublisher() : fuse_core::AsyncPublisher(1), device_id_(fuse_core::uuid::NIL), frame_id_("map") { } -void Path2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Path2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -77,103 +73,99 @@ void Path2DPublisher::onInit() { // Configure the publisher std::string device_str; - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); - if (device_str != "") { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::from_string(device_str); - } else { - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); - if (device_str != "") { + } + else + { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::generate(device_str); } } - frame_id_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); // Advertise the topic rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - path_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "path"), 1, pub_options); + path_publisher_ = rclcpp::create_publisher(interfaces_, fuse_core::joinTopicName(name_, "path"), + 1, pub_options); pose_array_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose_array"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "pose_array"), 1, pub_options); } -void Path2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr /*transaction*/, - fuse_core::Graph::ConstSharedPtr graph) +void Path2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, + fuse_core::Graph::ConstSharedPtr graph) { // Exit early if no one is listening - if ((path_publisher_->get_subscription_count() == 0) && - (pose_array_publisher_->get_subscription_count() == 0)) + if ((path_publisher_->get_subscription_count() == 0) && (pose_array_publisher_->get_subscription_count() == 0)) { return; } // Extract all of the 2D pose variables to the path std::vector poses; - for (const auto & variable : graph->getVariables()) { - auto orientation = dynamic_cast(&variable); - if (orientation && - (orientation->deviceId() == device_id_)) + for (const auto& variable : graph->getVariables()) + { + auto orientation = dynamic_cast(&variable); + if (orientation && (orientation->deviceId() == device_id_)) { - const auto & stamp = orientation->stamp(); + const auto& stamp = orientation->stamp(); auto position_uuid = fuse_variables::Position2DStamped(stamp, device_id_).uuid(); - if (!graph->variableExists(position_uuid)) { + if (!graph->variableExists(position_uuid)) + { continue; } - auto position = - dynamic_cast(&graph->getVariable(position_uuid)); + auto position = dynamic_cast(&graph->getVariable(position_uuid)); geometry_msgs::msg::PoseStamped pose; pose.header.stamp = stamp; pose.header.frame_id = frame_id_; pose.pose.position.x = position->x(); pose.pose.position.y = position->y(); pose.pose.position.z = 0.0; - pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); poses.push_back(std::move(pose)); } } // Exit if there are no poses - if (poses.empty()) { + if (poses.empty()) + { return; } // Sort the poses by timestamp - auto compare_stamps = - [](const geometry_msgs::msg::PoseStamped & pose1, const geometry_msgs::msg::PoseStamped & pose2) + auto compare_stamps = [](const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2) { + if (pose1.header.stamp.sec == pose2.header.stamp.sec) { - if (pose1.header.stamp.sec == pose2.header.stamp.sec) { - return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; - } else { - return pose1.header.stamp.sec < pose2.header.stamp.sec; - } - }; + return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; + } + else + { + return pose1.header.stamp.sec < pose2.header.stamp.sec; + } + }; std::sort(poses.begin(), poses.end(), compare_stamps); // Define the header for the aggregate message std_msgs::msg::Header header; header.stamp = poses.back().header.stamp; header.frame_id = frame_id_; // Convert the sorted poses into a Path msg - if (path_publisher_->get_subscription_count() > 0) { + if (path_publisher_->get_subscription_count() > 0) + { nav_msgs::msg::Path path_msg; path_msg.header = header; path_msg.poses = poses; path_publisher_->publish(path_msg); } // Convert the sorted poses into a PoseArray msg - if (pose_array_publisher_->get_subscription_count() > 0) { + if (pose_array_publisher_->get_subscription_count() > 0) + { geometry_msgs::msg::PoseArray pose_array_msg; pose_array_msg.header = header; - std::transform( - poses.begin(), - poses.end(), - std::back_inserter(pose_array_msg.poses), - [](const geometry_msgs::msg::PoseStamped & pose) - { - return pose.pose; - }); // NOLINT(whitespace/braces) + std::transform(poses.begin(), poses.end(), std::back_inserter(pose_array_msg.poses), + [](const geometry_msgs::msg::PoseStamped& pose) { return pose.pose; }); // NOLINT(whitespace/braces) pose_array_publisher_->publish(pose_array_msg); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index fe5c88dcc..5291c1aa8 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -61,38 +61,32 @@ PLUGINLIB_EXPORT_CLASS(fuse_publishers::Pose2DPublisher, fuse_core::Publisher); namespace { -bool findPose( - rclcpp::Logger logger, - rclcpp::Clock clock, - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & position_uuid, - geometry_msgs::msg::Pose & pose -) +bool findPose(rclcpp::Logger logger, rclcpp::Clock clock, const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& orientation_uuid, fuse_core::UUID& position_uuid, + geometry_msgs::msg::Pose& pose) { - try { + try + { orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); pose.position.x = position_variable.x(); pose.position.y = position_variable.y(); pose.position.z = 0.0; - pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger, clock, 10.0 * 1000, - "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); + pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger, clock, 10.0 * 1000, - "Failed to find a pose at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } return true; @@ -104,18 +98,17 @@ namespace fuse_publishers { Pose2DPublisher::Pose2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - publish_to_tf_(false), - tf_timeout_(rclcpp::Duration(0, 0)), - use_tf_lookup_(false) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , publish_to_tf_(false) + , tf_timeout_(rclcpp::Duration(0, 0)) + , use_tf_lookup_(false) { } -void Pose2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Pose2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -129,71 +122,66 @@ void Pose2DPublisher::onInit() tf_publisher_ = std::make_shared(interfaces_); // Read configuration from the parameter server - base_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "base_frame"), std::string("base_link")); - map_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "map_frame"), std::string("map")); - odom_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "odom_frame"), std::string("odom")); + base_frame_ = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "base_frame"), std::string("base_link")); + map_frame_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "map_frame"), std::string("map")); + odom_frame_ = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "odom_frame"), std::string("odom")); std::string device_str; - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); - if (device_str != "") { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::from_string(device_str); - } else { - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); - if (device_str != "") { + } + else + { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::generate(device_str); } } - publish_to_tf_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "publish_to_tf"), false); + publish_to_tf_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "publish_to_tf"), false); // Configure tf, if requested - if (publish_to_tf_) { + if (publish_to_tf_) + { use_tf_lookup_ = (!odom_frame_.empty() && (odom_frame_ != base_frame_)); - if (use_tf_lookup_) { + if (use_tf_lookup_) + { double tf_cache_time; double default_tf_cache_time = 10.0; - tf_cache_time = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_cache_time"), default_tf_cache_time); - if (tf_cache_time <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_cache_time is <= 0. Using the default value (" - << default_tf_cache_time << "s) instead."); + tf_cache_time = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_cache_time"), default_tf_cache_time); + if (tf_cache_time <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_cache_time is <= 0. Using the default value (" + << default_tf_cache_time << "s) instead."); tf_cache_time = default_tf_cache_time; } double tf_timeout; double default_tf_timeout = 0.1; - tf_timeout = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_timeout"), default_tf_timeout); - if (tf_timeout <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_timeout is <= 0. Using the default value (" - << default_tf_timeout << "s) instead."); + tf_timeout = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_timeout"), default_tf_timeout); + if (tf_timeout <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_timeout is <= 0. Using the default value (" << default_tf_timeout + << "s) instead."); tf_timeout = default_tf_timeout; } tf_timeout_ = rclcpp::Duration::from_seconds(tf_timeout); tf_buffer_ = std::make_unique( - clock_, - rclcpp::Duration::from_seconds(tf_cache_time) - .to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ // NOLINT - ); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() + clock_, rclcpp::Duration::from_seconds(tf_cache_time).to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ // NOLINT ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } } @@ -202,10 +190,9 @@ void Pose2DPublisher::onInit() pub_options.callback_group = cb_group_; pose_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose"), 1, pub_options); - pose_with_covariance_publisher_ = - rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose_with_covariance"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "pose"), 1, pub_options); + pose_with_covariance_publisher_ = rclcpp::create_publisher( + interfaces_, fuse_core::joinTopicName(name_, "pose_with_covariance"), 1, pub_options); } void Pose2DPublisher::onStart() @@ -215,64 +202,58 @@ void Pose2DPublisher::onStart() // Clear the synchronizer synchronizer_ = Synchronizer::make_unique(device_id_); // Start the tf timer - if (publish_to_tf_) { + if (publish_to_tf_) + { double tf_publish_frequency; double default_tf_publish_frequency = 10.0; // We pull the param again on each start so the publisher can technically get set to a different // publish frequency - tf_publish_frequency = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_publish_frequency"), - default_tf_publish_frequency); - if (tf_publish_frequency <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_publish_frequency is <= 0. Using the default value (" << - default_tf_publish_frequency << "hz) instead."); + tf_publish_frequency = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_publish_frequency"), + default_tf_publish_frequency); + if (tf_publish_frequency <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_publish_frequency is <= 0. Using the default value (" + << default_tf_publish_frequency << "hz) instead."); tf_publish_frequency = default_tf_publish_frequency; } - tf_publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / tf_publish_frequency), - std::move(std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)), - cb_group_ - ); + tf_publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / tf_publish_frequency), + std::move(std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)), cb_group_); } } void Pose2DPublisher::onStop() { // Stop the tf timer - if (publish_to_tf_) { + if (publish_to_tf_) + { tf_publish_timer_->cancel(); } } -void Pose2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Pose2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { auto latest_stamp = synchronizer_->findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { // If uninitialized - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of stamped pose variables with device id '" - << device_id_ << "'."); + if (0u == latest_stamp.nanoseconds()) + { // If uninitialized + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of stamped pose variables with device id '" << device_id_ + << "'."); return; } // Get the pose values associated with the selected timestamp fuse_core::UUID orientation_uuid; fuse_core::UUID position_uuid; geometry_msgs::msg::Pose pose; - if (!findPose( - logger_, *clock_, *graph, latest_stamp, device_id_, orientation_uuid, position_uuid, - pose)) + if (!findPose(logger_, *clock_, *graph, latest_stamp, device_id_, orientation_uuid, position_uuid, pose)) { return; } // Publish the various message types - if (publish_to_tf_) { + if (publish_to_tf_) + { // Create a 3D ROS Transform message from the current 2D pose geometry_msgs::msg::TransformStamped map_to_base; map_to_base.header.stamp = latest_stamp; @@ -284,37 +265,42 @@ void Pose2DPublisher::notifyCallback( map_to_base.transform.translation.z = pose.position.z; map_to_base.transform.rotation = pose.orientation; // If we are suppose to publish the map->odom frame instead, do that transformation now - if (use_tf_lookup_) { + if (use_tf_lookup_) + { // We need to lookup the base->odom frame first, so we can compute the map->odom transform // from the map->base transform - try { - auto base_to_odom = tf_buffer_->lookupTransform( - base_frame_, odom_frame_, latest_stamp, - tf_timeout_); + try + { + auto base_to_odom = tf_buffer_->lookupTransform(base_frame_, odom_frame_, latest_stamp, tf_timeout_); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, map_to_base); map_to_odom.child_frame_id = odom_frame_; // The child frame is not populated for some // reason tf_transform_ = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 2.0 * 1000, - "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ << ". Error: " - << e.what()); } - } else { + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 2.0 * 1000, + "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ + << ". Error: " << e.what()); + } + } + else + { // Simple. No intermediate frame. Just use the optimized map->base transform. tf_transform_ = map_to_base; } } - if (pose_publisher_->get_subscription_count() > 0) { + if (pose_publisher_->get_subscription_count() > 0) + { geometry_msgs::msg::PoseStamped msg; msg.header.stamp = latest_stamp; msg.header.frame_id = map_frame_; msg.pose = pose; pose_publisher_->publish(msg); } - if (pose_with_covariance_publisher_->get_subscription_count() > 0) { + if (pose_with_covariance_publisher_->get_subscription_count() > 0) + { // Get the covariance from the graph std::vector> requests; requests.emplace_back(position_uuid, position_uuid); @@ -343,7 +329,8 @@ void Pose2DPublisher::tfPublishTimerCallback() { // The tf_transform_ is updated in a separate thread, so we must guard the read/write operations. // Only publish if the tf transform is valid - if (rclcpp::Time(tf_transform_.header.stamp).nanoseconds()) { + if (rclcpp::Time(tf_transform_.header.stamp).nanoseconds()) + { // Update the timestamp of the transform so the tf tree will continue to be valid tf_transform_.header.stamp = clock_->now(); tf_publisher_->sendTransform(tf_transform_); diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 64d2b196f..5dad52ea4 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -50,17 +50,15 @@ namespace fuse_publishers { SerializedPublisher::SerializedPublisher() -: fuse_core::AsyncPublisher(1), - frame_id_("map"), - graph_publisher_throttled_callback_( - std::bind(&SerializedPublisher::graphPublisherCallback, this, std::placeholders::_1, - std::placeholders::_2)) + : fuse_core::AsyncPublisher(1) + , frame_id_("map") + , graph_publisher_throttled_callback_( + std::bind(&SerializedPublisher::graphPublisherCallback, this, std::placeholders::_1, std::placeholders::_2)) { } void SerializedPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -69,63 +67,52 @@ void SerializedPublisher::initialize( void SerializedPublisher::onInit() { // Configure the publisher - frame_id_ = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "frame_id"), frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); bool latch = false; - latch = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "latch"), latch); + latch = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "latch"), latch); - rclcpp::Duration graph_throttle_period{0, 0}; - fuse_core::getPositiveParam( - interfaces_, - fuse_core::joinParameterName(name_, "graph_throttle_period"), - graph_throttle_period, false); + rclcpp::Duration graph_throttle_period{ 0, 0 }; + fuse_core::getPositiveParam(interfaces_, fuse_core::joinParameterName(name_, "graph_throttle_period"), + graph_throttle_period, false); - bool graph_throttle_use_wall_time{false}; + bool graph_throttle_use_wall_time{ false }; graph_throttle_use_wall_time = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "graph_throttle_use_wall_time"), - graph_throttle_use_wall_time); + interfaces_, fuse_core::joinParameterName(name_, "graph_throttle_use_wall_time"), graph_throttle_use_wall_time); graph_publisher_throttled_callback_.setThrottlePeriod(graph_throttle_period); - if (!graph_throttle_use_wall_time) { - graph_publisher_throttled_callback_.setClock( - interfaces_.get_node_clock_interface()->get_clock()); + if (!graph_throttle_use_wall_time) + { + graph_publisher_throttled_callback_.setClock(interfaces_.get_node_clock_interface()->get_clock()); } // Advertise the topics rclcpp::QoS qos(1); // Queue size of 1 - if (latch) { + if (latch) + { qos.transient_local(); } rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - graph_publisher_ = - rclcpp::create_publisher( - interfaces_, "graph", qos, - pub_options); + graph_publisher_ = rclcpp::create_publisher(interfaces_, "graph", qos, pub_options); transaction_publisher_ = - rclcpp::create_publisher( - interfaces_, "transaction", qos, - pub_options); + rclcpp::create_publisher(interfaces_, "transaction", qos, pub_options); } -void SerializedPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void SerializedPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { - const auto & stamp = transaction->stamp(); - if (graph_publisher_->get_subscription_count() > 0) { + const auto& stamp = transaction->stamp(); + if (graph_publisher_->get_subscription_count() > 0) + { graph_publisher_throttled_callback_(graph, stamp); } - if (transaction_publisher_->get_subscription_count() > 0) { + if (transaction_publisher_->get_subscription_count() > 0) + { fuse_msgs::msg::SerializedTransaction msg; msg.header.stamp = stamp; msg.header.frame_id = frame_id_; @@ -134,8 +121,7 @@ void SerializedPublisher::notifyCallback( } } -void SerializedPublisher::graphPublisherCallback( - fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time & stamp) const +void SerializedPublisher::graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const { fuse_msgs::msg::SerializedGraph msg; msg.header.stamp = stamp; diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index 1c2026d3c..05ebcd059 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -53,7 +53,6 @@ #include #include - /** * @brief Test fixture for the Path2DPublisher * @@ -64,39 +63,33 @@ class Path2DPublisherTestFixture : public ::testing::Test { public: Path2DPublisherTestFixture() - : graph_(fuse_graphs::HashGraph::make_shared()), - transaction_(fuse_core::Transaction::make_shared()), - received_path_msg_(false), - received_pose_array_msg_(false) + : graph_(fuse_graphs::HashGraph::make_shared()) + , transaction_(fuse_core::Transaction::make_shared()) + , received_path_msg_(false) + , received_pose_array_msg_(false) { // Add a few pose variables - auto position1 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); + auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; transaction_->addInvolvedStamp(position1->stamp()); @@ -120,38 +113,34 @@ class Path2DPublisherTestFixture : public ::testing::Test mean1 << 1.01, 2.01, 3.01; fuse_core::Matrix3d cov1; /* *INDENT-OFF* */ - cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; + cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; /* *INDENT-ON* */ auto constraint1 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position1, *orientation1, mean1, cov1); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); fuse_core::Vector3d mean2; mean2 << 1.02, 2.02, 3.02; fuse_core::Matrix3d cov2; /* *INDENT-OFF* */ - cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; + cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; /* *INDENT-ON* */ auto constraint2 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position2, *orientation2, mean2, cov2); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position2, *orientation2, mean2, cov2); fuse_core::Vector3d mean3; mean3 << 1.03, 2.03, 3.03; fuse_core::Matrix3d cov3; /* *INDENT-OFF* */ - cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; + cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; /* *INDENT-ON* */ auto constraint3 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position3, *orientation3, mean3, cov3); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position3, *orientation3, mean3, cov3); fuse_core::Vector3d mean4; mean4 << 1.04, 2.04, 3.04; fuse_core::Matrix3d cov4; /* *INDENT-OFF* */ - cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; + cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; /* *INDENT-ON* */ auto constraint4 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position4, *orientation4, mean4, cov4); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position4, *orientation4, mean4, cov4); transaction_->addConstraint(constraint1); transaction_->addConstraint(constraint2); transaction_->addConstraint(constraint3); @@ -166,35 +155,33 @@ class Path2DPublisherTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - void pathCallback(const nav_msgs::msg::Path & msg) + void pathCallback(const nav_msgs::msg::Path& msg) { path_msg_ = msg; received_path_msg_ = true; } - void poseArrayCallback(const geometry_msgs::msg::PoseArray & msg) + void poseArrayCallback(const geometry_msgs::msg::PoseArray& msg) { pose_array_msg_ = msg; received_pose_array_msg_ = true; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; protected: @@ -210,10 +197,7 @@ TEST_F(Path2DPublisherTestFixture, PublishPath) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.frame_id:=test_map"}); + options.arguments({ "--ros-args", "-p", "test_publisher.frame_id:=test_map" }); auto node = rclcpp::Node::make_shared("test_path_2d_publisher_node", options); executor_->add_node(node); @@ -226,20 +210,20 @@ TEST_F(Path2DPublisherTestFixture, PublishPath) // Subscribe to the "path" topic auto subscriber1 = node->create_subscription( - "/test_publisher/path", 1, - std::bind(&Path2DPublisherTestFixture::pathCallback, this, std::placeholders::_1)); + "/test_publisher/path", 1, std::bind(&Path2DPublisherTestFixture::pathCallback, this, std::placeholders::_1)); // Subscribe to the "pose_array" topic auto subscriber2 = node->create_subscription( - "/test_publisher/pose_array", 1, - std::bind(&Path2DPublisherTestFixture::poseArrayCallback, this, std::placeholders::_1)); + "/test_publisher/pose_array", 1, + std::bind(&Path2DPublisherTestFixture::poseArrayCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_path_msg_) && (node->now() < timeout)) { + while ((!received_path_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index 947255cff..ee04675c8 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -55,7 +55,6 @@ #include - /** * @brief Test fixture for the LatestStampedPose2DPublisher * @@ -66,42 +65,34 @@ class Pose2DPublisherTestFixture : public ::testing::Test { public: Pose2DPublisherTestFixture() - : graph_(fuse_graphs::HashGraph::make_shared()), - transaction_(fuse_core::Transaction::make_shared()), - received_pose_msg_(false), - received_pose_with_covariance_msg_(false), - received_tf_msg_(false) + : graph_(fuse_graphs::HashGraph::make_shared()) + , transaction_(fuse_core::Transaction::make_shared()) + , received_pose_msg_(false) + , received_pose_with_covariance_msg_(false) + , received_tf_msg_(false) { // Add a few pose variables - auto position1 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), - fuse_core::uuid::generate("kitt")); + auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), - fuse_core::uuid::generate("kitt")); + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; transaction_->addInvolvedStamp(position1->stamp()); @@ -125,26 +116,26 @@ class Pose2DPublisherTestFixture : public ::testing::Test mean1 << 1.01, 2.01, 3.01; fuse_core::Matrix3d cov1; cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; - auto constraint1 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position1, *orientation1, mean1, cov1); + auto constraint1 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); fuse_core::Vector3d mean2; mean2 << 1.02, 2.02, 3.02; fuse_core::Matrix3d cov2; cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; - auto constraint2 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position2, *orientation2, mean2, cov2); + auto constraint2 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position2, *orientation2, mean2, cov2); fuse_core::Vector3d mean3; mean3 << 1.03, 2.03, 3.03; fuse_core::Matrix3d cov3; cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; - auto constraint3 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position3, *orientation3, mean3, cov3); + auto constraint3 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position3, *orientation3, mean3, cov3); fuse_core::Vector3d mean4; mean4 << 1.04, 2.04, 3.04; fuse_core::Matrix3d cov4; cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; - auto constraint4 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position4, *orientation4, mean4, cov4); + auto constraint4 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position4, *orientation4, mean4, cov4); transaction_->addConstraint(constraint1); transaction_->addConstraint(constraint2); transaction_->addConstraint(constraint3); @@ -172,41 +163,39 @@ class Pose2DPublisherTestFixture : public ::testing::Test rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - void poseCallback(const geometry_msgs::msg::PoseStamped & msg) + void poseCallback(const geometry_msgs::msg::PoseStamped& msg) { received_pose_msg_ = true; pose_msg_ = msg; } - void poseWithCovarianceCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) + void poseWithCovarianceCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { received_pose_with_covariance_msg_ = true; pose_with_covariance_msg_ = msg; } - void tfCallback(const tf2_msgs::msg::TFMessage & msg) + void tfCallback(const tf2_msgs::msg::TFMessage& msg) { received_tf_msg_ = true; tf_msg_ = msg; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; protected: @@ -225,13 +214,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishPose) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=false"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=false" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -242,15 +227,15 @@ TEST_F(Pose2DPublisherTestFixture, PublishPose) // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/test_publisher/pose", 1, - std::bind(&Pose2DPublisherTestFixture::poseCallback, this, std::placeholders::_1)); + "/test_publisher/pose", 1, std::bind(&Pose2DPublisherTestFixture::poseCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_msg_) && (node->now() < timeout)) { + while ((!received_pose_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -267,13 +252,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) { // Test that the expected PoseWithCovarianceStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=false"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=false" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -284,17 +265,16 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) // Subscribe to the "pose_with_covariance" topic auto subscriber1 = node->create_subscription( - "/test_publisher/pose_with_covariance", 1, - std::bind( - &Pose2DPublisherTestFixture::poseWithCovarianceCallback, this, - std::placeholders::_1)); + "/test_publisher/pose_with_covariance", 1, + std::bind(&Pose2DPublisherTestFixture::poseWithCovarianceCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_with_covariance_msg_) && (node->now() < timeout)) { + while ((!received_pose_with_covariance_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -305,18 +285,14 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) EXPECT_NEAR(2.02, pose_with_covariance_msg_.pose.pose.position.y, 1.0e-9); EXPECT_NEAR(0.00, pose_with_covariance_msg_.pose.pose.position.z, 1.0e-9); EXPECT_NEAR(3.02, tf2::getYaw(pose_with_covariance_msg_.pose.pose.orientation), 1.0e-9); - std::vector expected_covariance = - { + std::vector expected_covariance = { /* *INDENT-OFF* */ - 1.02, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 2.02, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 3.02 + 1.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, + 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 3.02 /* *INDENT-ON* */ }; - for (size_t i = 0; i < 36; ++i) { + for (size_t i = 0; i < 36; ++i) + { EXPECT_NEAR(expected_covariance[i], pose_with_covariance_msg_.pose.covariance[i], 1.0e-9); } } @@ -325,13 +301,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) { // Test that the expected TFMessage is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_base", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=true"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_base", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=true" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -342,8 +314,7 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/tf", 1, - std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); + "/tf", 1, std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); // Create a publisher and send it the graph fuse_publishers::Pose2DPublisher publisher; @@ -355,7 +326,8 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node->now() < timeout)) { + while ((!received_tf_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -373,20 +345,15 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) { // Test that the expected TFMessage is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=true"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=true" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/tf", 1, - std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); + "/tf", 1, std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); auto tf_node_ = rclcpp::Node::make_shared("tf_pub_node"); executor_->add_node(tf_node_); @@ -403,7 +370,8 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node->now() < timeout)) { + while ((!received_tf_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } diff --git a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp index 03acb1a6d..4a2716ad3 100644 --- a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp +++ b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp @@ -68,47 +68,25 @@ TEST(StampedVariableSynchronizer, FullSearch) // timestamp has been found before // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the transaction and graph auto transaction = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("Dadblank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("Dadblank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual = sync.findLatestCommonStamp(transaction, graph); @@ -124,32 +102,19 @@ TEST(StampedVariableSynchronizer, Update) // Perform an initial search, then use the transaction to perform an incremental update // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); @@ -158,35 +123,17 @@ TEST(StampedVariableSynchronizer, Update) // Create an incremental transaction update auto transaction2 = fuse_core::Transaction(); transaction2.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); transaction2.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); transaction2.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); @@ -197,42 +144,23 @@ TEST(StampedVariableSynchronizer, Remove) { // Perform an initial search, then use the transaction to remove the latest variables // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); @@ -241,13 +169,8 @@ TEST(StampedVariableSynchronizer, Remove) // Create an incremental transaction that removes one of the latest variables auto transaction2 = fuse_core::Transaction(); transaction2.removeVariable( - fuse_variables::Position2DStamped( - rclcpp::Time(30, 0, RCL_ROS_TIME), - generate("blank")).uuid()); - graph.removeVariable( - fuse_variables::Position2DStamped( - rclcpp::Time(30, 0, RCL_ROS_TIME), - generate("blank")).uuid()); + fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); + graph.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); diff --git a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp index 475407f28..226782169 100644 --- a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp +++ b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_tutorials { /** @@ -121,8 +120,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * queue. Generally this will be 1, unless you have a good reason to use a multi-threaded * executor. */ - BeaconPublisher() - : fuse_core::AsyncPublisher(1) {} + BeaconPublisher() : fuse_core::AsyncPublisher(1) + { + } /** * @brief Shadowing extension to the AsyncPublisher::initialize call @@ -130,9 +130,8 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * This allows you to bind more node interfaces to the publisher than the ones supported by the * base AsyncPublisher class. */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required initialization for the publisher @@ -152,7 +151,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * so there is nothing to do here. If this was not a tutorial, I would have omitted the override * entirely. */ - void onStart() override {} + void onStart() override + { + } /** * @brief Perform any required operations to stop publications @@ -162,7 +163,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * cleanup, so there is nothing to do here. If this was not a tutorial, I would have omitted the * override entirely. */ - void onStop() override {} + void onStop() override + { + } /** * @brief Called when a newly optimized graph has been generated by the optimizer @@ -179,18 +182,14 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * @param[in] graph - A read-only pointer to the graph object, allowing queries to be performed * whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ std::string map_frame_id_; //!< The name of the robot's map frame diff --git a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp index f59f2a475..a9a6e6b64 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp @@ -48,7 +48,6 @@ #include #include - namespace fuse_tutorials { /** @@ -115,12 +114,8 @@ class RangeConstraint : public fuse_core::Constraint * @param[in] z - The distance measured between the robot and beacon by our new sensor * @param[in] sigma - The uncertainty of measured distance */ - RangeConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & robot_position, - const fuse_variables::Point2DLandmark & beacon_position, - const double z, - const double sigma); + RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, + const fuse_variables::Point2DLandmark& beacon_position, const double z, const double sigma); /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -130,7 +125,7 @@ class RangeConstraint : public fuse_core::Constraint * * @param[out] stream - The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -146,7 +141,7 @@ class RangeConstraint : public fuse_core::Constraint * CostFunction object when it is done. Also note that the fuse project guarantees the * derived Constraint object will outlive the Ceres Solver CostFunction object. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; private: // Allow Boost Serialization access to private methods and members @@ -167,16 +162,16 @@ class RangeConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & sigma_; - archive & z_; + archive& boost::serialization::base_object(*this); + archive& sigma_; + archive& z_; } - double sigma_ {0.0}; //!< The standard deviation of the range measurement - double z_ {0.0}; //!< The measured range to the beacon + double sigma_{ 0.0 }; //!< The standard deviation of the range measurement + double z_{ 0.0 }; //!< The measured range to the beacon }; } // namespace fuse_tutorials diff --git a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp index 4da4f2c06..3a2920796 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp @@ -89,8 +89,9 @@ class RangeCostFunctor * @param[in] z The measured range to the beacon * @param[in] sigma The standard deviation of the range measurement */ - RangeCostFunctor(const double z, const double sigma) - : sigma_(sigma), z_(z) {} + RangeCostFunctor(const double z, const double sigma) : sigma_(sigma), z_(z) + { + } /** * @brief Compute the costs using the provided stored measurement details and the provided @@ -120,10 +121,8 @@ class RangeCostFunctor * only a single value. * @return True if the cost was computed successfully, false otherwise */ - template - bool operator()( - const T * const robot_position, const T * const beacon_position, - T * residuals) const + template + bool operator()(const T* const robot_position, const T* const beacon_position, T* residuals) const { // Implement our mathematic measurement model: // z_hat = sqrt( (x_robot - x_beacon)^2 + (y_robot - y_beacon)^2 ) @@ -135,10 +134,13 @@ class RangeCostFunctor auto dx = robot_position[0] - beacon_position[0]; auto dy = robot_position[1] - beacon_position[1]; auto norm_sq = dx * dx + dy * dy; - if (norm_sq > 0.0) { + if (norm_sq > 0.0) + { auto z_hat = ceres::sqrt(norm_sq); residuals[0] = (T(z_) - z_hat) / T(sigma_); - } else { + } + else + { residuals[0] = T(z_) / T(sigma_); } return true; @@ -146,7 +148,7 @@ class RangeCostFunctor private: double sigma_; //!< The standard deviation of the range measurement - double z_; //!< The measured range to the beacon + double z_; //!< The measured range to the beacon }; } // namespace fuse_tutorials diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp index 1e903dead..52e5baa99 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_tutorials { /** @@ -129,16 +128,15 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * optimizer node. We do, however, specify the number of threads to use to spin the callback * queue. Generally this will be 1, unless you have a good reason to use a multi-threaded spinner. */ - RangeSensorModel() - : fuse_core::AsyncSensorModel(1), logger_(rclcpp::get_logger("uninitialized")) {} + RangeSensorModel() : fuse_core::AsyncSensorModel(1), logger_(rclcpp::get_logger("uninitialized")) + { + } /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Receives the set of known beacon positions @@ -151,7 +149,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - Message containing the database of known but noisy beacon positions. */ - void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 & msg); + void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg); /** * @brief Callback for range measurement messages @@ -162,7 +160,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - The range message to process */ - void rangesCallback(const sensor_msgs::msg::PointCloud2 & msg); + void rangesCallback(const sensor_msgs::msg::PointCloud2& msg); protected: /** @@ -192,12 +190,10 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel double sigma; }; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Logger logger_; //!< The sensor model's logger @@ -205,7 +201,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel //!< ROS subscription for the database of prior beacon positions rclcpp::Subscription::SharedPtr beacon_sub_; - bool initialized_ {false}; //!< Flag indicating the initial beacon positions have been processed + bool initialized_{ false }; //!< Flag indicating the initial beacon positions have been processed //!< ROS subscription for the range sensor measurements rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index b68bd35a8..4317e096b 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -49,12 +49,10 @@ // Register this publisher with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_tutorials::BeaconPublisher, fuse_core::Publisher); - namespace fuse_tutorials { -void BeaconPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void BeaconPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -72,20 +70,21 @@ void BeaconPublisher::onInit() pub_options.callback_group = cb_group_; beacon_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "beacons"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "beacons"), 1, pub_options); } -void BeaconPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr /* transaction */, - fuse_core::Graph::ConstSharedPtr graph) +void BeaconPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /* transaction */, + fuse_core::Graph::ConstSharedPtr graph) { // This is where all of the processing happens in this publisher implementation. All of the // beacons are represented as fuse_variables::Point2DLandmark objects. We loop through the // variables in the graph and keep a pointer to the variables that are the correct type. - auto beacons = std::vector(); - for (const auto & variable : graph->getVariables()) { - const auto beacon = dynamic_cast(&variable); - if (beacon) { + auto beacons = std::vector(); + for (const auto& variable : graph->getVariables()) + { + const auto beacon = dynamic_cast(&variable); + if (beacon) + { beacons.push_back(beacon); } } @@ -110,8 +109,9 @@ void BeaconPublisher::notifyCallback( sensor_msgs::PointCloud2Iterator y_it(msg, "y"); sensor_msgs::PointCloud2Iterator z_it(msg, "z"); sensor_msgs::PointCloud2Iterator id_it(msg, "id"); - for (auto id = 0u; id < beacons.size(); ++id) { - const auto & beacon = beacons.at(id); + for (auto id = 0u; id < beacons.size(); ++id) + { + const auto& beacon = beacons.at(id); *x_it = static_cast(beacon->x()); *y_it = static_cast(beacon->y()); *z_it = 0.0f; diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp index 9760bfcb9..de4f8887f 100644 --- a/fuse_tutorials/src/range_constraint.cpp +++ b/fuse_tutorials/src/range_constraint.cpp @@ -51,19 +51,17 @@ namespace fuse_tutorials // variable order defined in the RangeCostFunctor must match the variable order provided to the base // class Constraint constructor. In this case, robot position, then the beacon position // fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) -RangeConstraint::RangeConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & robot_position, - const fuse_variables::Point2DLandmark & beacon_position, - const double z, - const double sigma) -: fuse_core::Constraint(source, {robot_position.uuid(), beacon_position.uuid()}), // NOLINT - sigma_(sigma), - z_(z) +RangeConstraint::RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, + const fuse_variables::Point2DLandmark& beacon_position, const double z, + const double sigma) + : fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) + , // NOLINT + sigma_(sigma) + , z_(z) { } -void RangeConstraint::print(std::ostream & stream) const +void RangeConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -74,7 +72,7 @@ void RangeConstraint::print(std::ostream & stream) const << " range sigma: " << sigma_ << "\n"; } -ceres::CostFunction * RangeConstraint::costFunction() const +ceres::CostFunction* RangeConstraint::costFunction() const { // Here we use the Ceres Solver AutoDiffCostFunction class to generate a derived CostFunction // object from our RangeCostFunctor. The AutoDiffCostFunction requires the cost functor as the @@ -87,8 +85,7 @@ ceres::CostFunction * RangeConstraint::costFunction() const // is also 2. // If there were additional involved variables, the size of each variable would appear here in // order. - return new ceres::AutoDiffCostFunction( - new RangeCostFunctor(z_, sigma_)); + return new ceres::AutoDiffCostFunction(new RangeCostFunctor(z_, sigma_)); } } // namespace fuse_tutorials diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 9991adc62..89305d04d 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -48,16 +48,14 @@ PLUGINLIB_EXPORT_CLASS(fuse_tutorials::RangeSensorModel, fuse_core::SensorModel) namespace fuse_tutorials { -void RangeSensorModel::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void RangeSensorModel::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); } -void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 & msg) +void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg) { // Store a copy of the beacon database. We use a map to allow efficient lookups by ID number. sensor_msgs::PointCloud2ConstIterator x_it(msg, "x"); @@ -65,8 +63,9 @@ void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 sensor_msgs::PointCloud2ConstIterator z_it(msg, "z"); sensor_msgs::PointCloud2ConstIterator sigma_it(msg, "sigma"); sensor_msgs::PointCloud2ConstIterator id_it(msg, "id"); - for (; x_it != x_it.end(); ++x_it, ++y_it, ++z_it, ++sigma_it, ++id_it) { - beacon_db_[*id_it] = Beacon {*x_it, *y_it, *sigma_it}; + for (; x_it != x_it.end(); ++x_it, ++y_it, ++z_it, ++sigma_it, ++id_it) + { + beacon_db_[*id_it] = Beacon{ *x_it, *y_it, *sigma_it }; } RCLCPP_INFO_STREAM(logger_, "Updated Beacon Database."); } @@ -86,14 +85,8 @@ void RangeSensorModel::onInit() latched_qos.transient_local(); beacon_sub_ = rclcpp::create_subscription( - interfaces_, - "prior_beacons", - latched_qos, - std::bind( - &RangeSensorModel::priorBeaconsCallback, this, std::placeholders::_1 - ), - sub_options - ); + interfaces_, "prior_beacons", latched_qos, + std::bind(&RangeSensorModel::priorBeaconsCallback, this, std::placeholders::_1), sub_options); } void RangeSensorModel::onStart() @@ -112,14 +105,7 @@ void RangeSensorModel::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - "ranges", - 10, - std::bind( - &RangeSensorModel::rangesCallback, this, std::placeholders::_1 - ), - sub_options - ); + interfaces_, "ranges", 10, std::bind(&RangeSensorModel::rangesCallback, this, std::placeholders::_1), sub_options); } void RangeSensorModel::onStop() @@ -131,7 +117,7 @@ void RangeSensorModel::onStop() sub_.reset(); } -void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) +void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2& msg) { // We received a new message for our sensor. This is where most of the processing happens for our // sensor model. We take the published ROS message and transform it into one or more Constraints, @@ -139,7 +125,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // However, if we have not received the prior beacon positions yet, we cannot process the range // messages. - if (beacon_db_.empty()) { + if (beacon_db_.empty()) + { return; } @@ -169,7 +156,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) sensor_msgs::PointCloud2ConstIterator id_it(msg, "id"); sensor_msgs::PointCloud2ConstIterator range_it(msg, "range"); sensor_msgs::PointCloud2ConstIterator sigma_it(msg, "sigma"); - for (; id_it != id_it.end(); ++id_it, ++range_it, ++sigma_it) { + for (; id_it != id_it.end(); ++id_it, ++range_it, ++sigma_it) + { // Each measure range will involve a different observed beacon. Construct a variable for this // measurement's beacon. auto beacon = beacon_db_[*id_it]; @@ -180,12 +168,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // Now that we have the involved variables defined, create a constraint for this sensor // measurement - auto constraint = fuse_tutorials::RangeConstraint::make_shared( - this->name(), - *robot_position, - *beacon_position, - *range_it, - *sigma_it); + auto constraint = fuse_tutorials::RangeConstraint::make_shared(this->name(), *robot_position, *beacon_position, + *range_it, *sigma_it); transaction->addConstraint(constraint); // If this is the very first measurement, add a prior position constraint on all of the beacons @@ -196,15 +180,14 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // beacons could be tracked over multiple samples. Once the beacon has been observed with enough // parallax, then all measurements of that beacon could be added at once. Some type of delayed // initialization scheme is common in vision-based odometry and SLAM systems. - if (!initialized_) { + if (!initialized_) + { auto mean = fuse_core::Vector2d(beacon.x, beacon.y); auto cov = fuse_core::Matrix2d(); cov << beacon.sigma * beacon.sigma, 0.0, 0.0, beacon.sigma * beacon.sigma; /* *INDENT-OFF* */ - auto prior = - fuse_constraints::AbsoluteConstraint::make_shared( - this->name(), *beacon_position, mean, cov - ); + auto prior = fuse_constraints::AbsoluteConstraint::make_shared( + this->name(), *beacon_position, mean, cov); /* *INDENT-ON* */ transaction->addConstraint(prior); } diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index 360e7f7ad..89cb88d52 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -45,25 +45,25 @@ #include #include -static constexpr double SITE_WIDTH = 100.0; //!< The width/length of the test area in meters -static constexpr double BEACON_SPACING = 20.0; //!< The distance between each range beacon -static constexpr double BEACON_SIGMA = 4.0; //!< Std dev used to create the database of noisy - //!< beacon positions +static constexpr double SITE_WIDTH = 100.0; //!< The width/length of the test area in meters +static constexpr double BEACON_SPACING = 20.0; //!< The distance between each range beacon +static constexpr double BEACON_SIGMA = 4.0; //!< Std dev used to create the database of noisy + //!< beacon positions static constexpr double ROBOT_PATH_RADIUS = 0.35 * SITE_WIDTH; //!< The radius of the simulated //!< robot's path -static constexpr double ROBOT_VELOCITY = 10.0; //!< The forward velocity of our simulated robot -static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel - //!< odometry data -static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth - //!< data -static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise -static constexpr double ODOM_VX_SIGMA = 0.5; //!< Std dev of simulated Odometry linear velocity - //!< measurement noise -static constexpr double ODOM_VYAW_SIGMA = 0.5; //!< Std dev of simulated Odometry angular velocity - //!< measurement noise -static constexpr double RANGE_SIGMA = 0.5; //!< Std dev of simulated beacon range measurement noise +static constexpr double ROBOT_VELOCITY = 10.0; //!< The forward velocity of our simulated robot +static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel + //!< odometry data +static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise +static constexpr double ODOM_VX_SIGMA = 0.5; //!< Std dev of simulated Odometry linear velocity + //!< measurement noise +static constexpr double ODOM_VYAW_SIGMA = 0.5; //!< Std dev of simulated Odometry angular velocity + //!< measurement noise +static constexpr double RANGE_SIGMA = 0.5; //!< Std dev of simulated beacon range measurement noise /** * @brief The position of a "range beacon" @@ -94,9 +94,11 @@ struct Robot std::vector createBeacons() { auto beacons = std::vector(); - for (auto x = -SITE_WIDTH / 2; x <= SITE_WIDTH / 2; x += BEACON_SPACING) { - for (auto y = -SITE_WIDTH / 2; y <= SITE_WIDTH / 2; y += BEACON_SPACING) { - beacons.push_back({x, y}); // NOLINT[whitespace/braces] + for (auto x = -SITE_WIDTH / 2; x <= SITE_WIDTH / 2; x += BEACON_SPACING) + { + for (auto y = -SITE_WIDTH / 2; y <= SITE_WIDTH / 2; y += BEACON_SPACING) + { + beacons.push_back({ x, y }); // NOLINT[whitespace/braces] } } return beacons; @@ -105,15 +107,16 @@ std::vector createBeacons() /** * @brief Create a noisy set of beacon priors from the true set of beacons */ -std::vector createNoisyBeacons(const std::vector & beacons) +std::vector createNoisyBeacons(const std::vector& beacons) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, BEACON_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, BEACON_SIGMA }; auto noisy_beacons = std::vector(); - for (const auto & beacon : beacons) { - noisy_beacons.push_back({beacon.x + noise(generator), beacon.y + noise(generator)}); // NOLINT + for (const auto& beacon : beacons) + { + noisy_beacons.push_back({ beacon.x + noise(generator), beacon.y + noise(generator) }); // NOLINT } return noisy_beacons; } @@ -121,36 +124,37 @@ std::vector createNoisyBeacons(const std::vector & beacons) /** * @brief Convert the set of beacons into a pointcloud for visualization purposes */ -sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud( - const std::vector & beacons, - const rclcpp::Clock & clock) +sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(const std::vector& beacons, + const rclcpp::Clock& clock) { auto msg = std::make_shared(); msg->header.stamp = clock.now(); msg->header.frame_id = MAP_FRAME; sensor_msgs::PointCloud2Modifier modifier(*msg); - modifier.setPointCloud2Fields( - 5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, - "y", 1, sensor_msgs::msg::PointField::FLOAT32, - "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "sigma", 1, sensor_msgs::msg::PointField::FLOAT32, - "id", 1, sensor_msgs::msg::PointField::UINT32); + modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "sigma", 1, sensor_msgs::msg::PointField::FLOAT32, "id", 1, + sensor_msgs::msg::PointField::UINT32); modifier.resize(beacons.size()); sensor_msgs::PointCloud2Iterator x_it(*msg, "x"); sensor_msgs::PointCloud2Iterator y_it(*msg, "y"); sensor_msgs::PointCloud2Iterator z_it(*msg, "z"); sensor_msgs::PointCloud2Iterator sigma_it(*msg, "sigma"); sensor_msgs::PointCloud2Iterator id_it(*msg, "id"); - for (auto id = 0u; id < beacons.size(); ++id) { + for (auto id = 0u; id < beacons.size(); ++id) + { // Compute the distance to each beacon - const auto & beacon = beacons.at(id); + const auto& beacon = beacons.at(id); *x_it = static_cast(beacon.x); *y_it = static_cast(beacon.y); *z_it = 10.0f; *sigma_it = static_cast(BEACON_SIGMA); *id_it = id; - ++x_it; ++y_it; ++z_it; ++sigma_it, ++id_it; + ++x_it; + ++y_it; + ++z_it; + ++sigma_it, ++id_it; } return msg; } @@ -158,7 +162,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud( /** * @brief Convert the robot state into a ground truth odometry message */ -nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) { auto msg = std::make_shared(); msg->header.stamp = state.stamp; @@ -197,11 +201,8 @@ nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) * * The state estimator will not run until it has been sent a starting pose. */ -void initializeStateEstimation( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const Robot & state, - const rclcpp::Clock::SharedPtr & clock, - const rclcpp::Logger & logger) +void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) { // Send the initial localization signal to the state estimator auto srv = std::make_shared(); @@ -220,29 +221,26 @@ void initializeStateEstimation( srv->pose.pose.covariance[28] = 1.0; srv->pose.pose.covariance[35] = 1.0; - auto client = rclcpp::create_client( - interfaces.get_node_base_interface(), - interfaces.get_node_graph_interface(), - interfaces.get_node_services_interface(), - "/state_estimation/set_pose", - rclcpp::ServicesQoS() - ); + auto client = rclcpp::create_client(interfaces.get_node_base_interface(), + interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), + "/state_estimation/set_pose", rclcpp::ServicesQoS()); while (!client->wait_for_service(std::chrono::seconds(30)) && - interfaces.get_node_base_interface()->get_context()->is_valid()) + interfaces.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); } auto success = false; - while (!success) { + while (!success) + { clock->sleep_for(std::chrono::milliseconds(100)); srv->pose.header.stamp = clock->now(); auto result_future = client->async_send_request(srv); if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future) != - rclcpp::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(logger, "service call failed :("); client->remove_pending_request(result_future); @@ -255,7 +253,7 @@ void initializeStateEstimation( /** * @brief Compute the next robot state given the current robot state and a simulated step time */ -Robot simulateRobotMotion(const Robot & previous_state, const rclcpp::Time & now) +Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now) { auto dt = (now - previous_state.stamp).seconds(); auto theta = std::atan2(previous_state.y, previous_state.x) + (dt * previous_state.vyaw); @@ -273,11 +271,11 @@ Robot simulateRobotMotion(const Robot & previous_state, const rclcpp::Time & now /** * @brief Create a simulated Imu measurement from the current state */ -sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, IMU_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -292,12 +290,12 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) /** * @brief Create a simulated Odometry measurement from the current state */ -nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot & robot) +nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot& robot) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> vx_noise{0.0, ODOM_VX_SIGMA}; - static std::normal_distribution<> vyaw_noise{0.0, ODOM_VYAW_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> vx_noise{ 0.0, ODOM_VX_SIGMA }; + static std::normal_distribution<> vyaw_noise{ 0.0, ODOM_VYAW_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -312,13 +310,11 @@ nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot & robot) return msg; } -sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( - const Robot & robot, - const std::vector & beacons) +sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor(const Robot& robot, const std::vector& beacons) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, RANGE_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, RANGE_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -326,19 +322,19 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( // Configure the pointcloud to have the following fields: id, range, sigma sensor_msgs::PointCloud2Modifier modifier(*msg); - modifier.setPointCloud2Fields( - 3, "id", 1, sensor_msgs::msg::PointField::UINT32, - "range", 1, sensor_msgs::msg::PointField::FLOAT64, - "sigma", 1, sensor_msgs::msg::PointField::FLOAT64); + modifier.setPointCloud2Fields(3, "id", 1, sensor_msgs::msg::PointField::UINT32, "range", 1, + sensor_msgs::msg::PointField::FLOAT64, "sigma", 1, + sensor_msgs::msg::PointField::FLOAT64); // Generate the simulated range to each known beacon modifier.resize(beacons.size()); sensor_msgs::PointCloud2Iterator id_it(*msg, "id"); sensor_msgs::PointCloud2Iterator range_it(*msg, "range"); sensor_msgs::PointCloud2Iterator sigma_it(*msg, "sigma"); - for (auto id = 0u; id < beacons.size(); ++id) { + for (auto id = 0u; id < beacons.size(); ++id) + { // Compute the distance to each beacon - const auto & beacon = beacons.at(id); + const auto& beacon = beacons.at(id); auto dx = robot.x - beacon.x; auto dy = robot.y - beacon.y; auto range = std::sqrt(dx * dx + dy * dy) + noise(generator); @@ -346,7 +342,9 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( *id_it = id; *range_it = range; *sigma_it = RANGE_SIGMA; - ++id_it; ++range_it; ++sigma_it; + ++id_it; + ++range_it; + ++sigma_it; } return msg; } @@ -374,7 +372,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( * - The true position and velocity of the robot is published as a nav_msgs::msg::Odometry message * on the /ground_truth topic at 10Hz */ -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("range_sensor_simulator"); @@ -384,18 +382,12 @@ int main(int argc, char ** argv) auto latched_qos = rclcpp::QoS(1); latched_qos.transient_local(); - auto imu_publisher = - node->create_publisher("imu", 1); - auto true_beacons_publisher = - node->create_publisher("true_beacons", latched_qos); - auto prior_beacons_publisher = - node->create_publisher("prior_beacons", latched_qos); - auto wheel_odom_publisher = - node->create_publisher("wheel_odom", 1); - auto ground_truth_publisher = - node->create_publisher("ground_truth", 1); - auto range_publisher = - node->create_publisher("ranges", 1); + auto imu_publisher = node->create_publisher("imu", 1); + auto true_beacons_publisher = node->create_publisher("true_beacons", latched_qos); + auto prior_beacons_publisher = node->create_publisher("prior_beacons", latched_qos); + auto wheel_odom_publisher = node->create_publisher("wheel_odom", 1); + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + auto range_publisher = node->create_publisher("ranges", 1); // Create the true set of range beacons auto beacons = createBeacons(); @@ -420,7 +412,8 @@ int main(int argc, char ** argv) // Simulate the robot traveling in a circular path auto rate = rclcpp::Rate(10.0); - while (rclcpp::ok()) { + while (rclcpp::ok()) + { // Simulate the robot motion auto new_state = simulateRobotMotion(state, node->now()); // Publish the new ground truth 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 b9d9e407c..e4e32b8a6 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_angular_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_angular_2d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index bc9f1714b..efaeeb0f9 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -86,26 +86,31 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular2DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular acceleration. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the angular acceleration. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -113,7 +118,10 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -127,11 +135,11 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 6c5b6fba1..26c2d3346 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_angular_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_angular_3d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index b43683cfc..b91b1e1b5 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -88,46 +88,63 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular3DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular acceleration. */ - double & roll() {return data_[ROLL];} + double& roll() + { + return data_[ROLL]; + } /** * @brief Read-only access to the roll (X-axis) angular acceleration. */ - const double & roll() const {return data_[ROLL];} + const double& roll() const + { + return data_[ROLL]; + } /** * @brief Read-write access to the pitch (Y-axis) angular acceleration. */ - double & pitch() {return data_[PITCH];} + double& pitch() + { + return data_[PITCH]; + } /** * @brief Read-only access to the pitch (Y-axis) angular acceleration. */ - const double & pitch() const {return data_[PITCH];} + const double& pitch() const + { + return data_[PITCH]; + } /** * @brief Read-write access to the yaw (Z-axis) angular acceleration. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the yaw (Z-axis) angular acceleration. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +152,10 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +169,11 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 f587aa099..52287c600 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_linear_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_linear_2d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index ae7a539a8..cbdf503cd 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -87,36 +87,47 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear2DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear acceleration. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear acceleration. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -124,7 +135,10 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -138,11 +152,11 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 6808f6b0f..b330ecfd8 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_linear_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_linear_3d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index c48e45a81..11c71edaa 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -88,46 +88,63 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear3DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear acceleration. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear acceleration. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis linear acceleration. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis linear acceleration. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +152,10 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +169,11 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp index e2451569c..1d4e86f66 100644 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp +++ b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_variables { @@ -60,7 +59,7 @@ namespace fuse_variables * of typical variable types (points, poses, calibration parameters) are all known at design/compile * time. */ -template +template class FixedSizeVariable : public fuse_core::Variable { public: @@ -79,10 +78,10 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief Constructor */ - explicit FixedSizeVariable(const fuse_core::UUID & uuid) - : fuse_core::Variable(uuid), - data_{} // zero-initialize the data array - {} + explicit FixedSizeVariable(const fuse_core::UUID& uuid) + : fuse_core::Variable(uuid), data_{} // zero-initialize the data array + { + } /** * @brief Destructor @@ -95,27 +94,42 @@ class FixedSizeVariable : public fuse_core::Variable * The number of scalar values contained by this variable type is defined by the class template * parameter \p N. */ - size_t size() const override {return N;} + size_t size() const override + { + return N; + } /** * @brief Read-only access to the variable data */ - const double * data() const override {return data_.data();} + const double* data() const override + { + return data_.data(); + } /** * @brief Read-write access to the variable data */ - double * data() override {return data_.data();} + double* data() override + { + return data_.data(); + } /** * @brief Read-only access to the variable data as a std::array */ - const std::array & array() const {return data_;} + const std::array& array() const + { + return data_; + } /** * @brief Read-write access to the variable data as a std::array */ - std::array & array() {return data_;} + std::array& array() + { + return data_; + } protected: std::array data_; //!< Fixed-sized, contiguous memory for holding the variable data @@ -131,16 +145,16 @@ class FixedSizeVariable : public fuse_core::Variable * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; // Define the constant that was declared above -template +template constexpr size_t FixedSizeVariable::SIZE; } // namespace fuse_variables diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 987d64712..6393926f0 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -78,37 +78,27 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati 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 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; @@ -125,10 +115,10 @@ 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 - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -145,31 +135,37 @@ class Orientation2DManifold : public fuse_core::Manifold public: FUSE_SMART_PTR_DEFINITIONS(Orientation2DManifold) - int AmbientSize() const override {return 1;} + int AmbientSize() const override + { + return 1; + } - int TangentSize() const override {return 1;} + int TangentSize() 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 PlusJacobian(const double * /*x*/, double * jacobian) const override + bool PlusJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { // Compute the difference from y to x, and handle the 2*Pi rollover y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } - bool MinusJacobian(const double * /*x*/, double * jacobian) const override + bool MinusJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -185,10 +181,10 @@ class Orientation2DManifold : public fuse_core::Manifold * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; #endif @@ -226,26 +222,30 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the heading angle. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the heading angle. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Returns the number of elements of the local parameterization space. @@ -253,7 +253,10 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * Since we are overriding the \p localParameterization() method, it is good practice to override * the \p localSize() method as well. */ - size_t localSize() const override {return 1u;} + size_t localSize() const override + { + return 1u; + } /** * @brief Create a new Ceres local parameterization object to apply to updates of this variable @@ -263,7 +266,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived LocalParameterization */ - fuse_core::LocalParameterization * localParameterization() const override; + fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -274,7 +277,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived manifold */ - fuse_core::Manifold * manifold() const override; + fuse_core::Manifold* manifold() const override; #endif private: @@ -288,11 +291,11 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 412e19a4f..1781597c1 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -63,7 +63,7 @@ namespace fuse_variables * * ceres/rotation.h is missing this function for some reason. */ -template +template inline static void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; @@ -85,14 +85,17 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati public: FUSE_SMART_PTR_DEFINITIONS(Orientation3DLocalParameterization) - int GlobalSize() const override {return 4;} + int GlobalSize() const override + { + return 4; + } - int LocalSize() const override {return 3;} + int LocalSize() const override + { + return 3; + } - bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -100,27 +103,30 @@ 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; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus( - const double * x, - const double * y, - double * y_minus_x) const override + bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -130,18 +136,25 @@ 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; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -157,10 +170,10 @@ class Orientation3DLocalParameterization : 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 - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -177,11 +190,17 @@ class Orientation3DManifold : public fuse_core::Manifold public: FUSE_SMART_PTR_DEFINITIONS(Orientation3DManifold) - int AmbientSize() const override {return 4;} + int AmbientSize() const override + { + return 4; + } - int TangentSize() const override {return 3;} + int TangentSize() const override + { + return 3; + } - 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 { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -189,22 +208,30 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool PlusJacobian(const double * x, double * jacobian) const override + bool PlusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -214,16 +241,25 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool MinusJacobian(const double * x, double * jacobian) const override + bool MinusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -238,10 +274,10 @@ class Orientation3DManifold : public fuse_core::Manifold * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; #endif @@ -295,71 +331,102 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the quaternion w component */ - double & w() {return data_[W];} + double& w() + { + return data_[W]; + } /** * @brief Read-only access to the quaternion w component */ - const double & w() const {return data_[W];} + const double& w() const + { + return data_[W]; + } /** * @brief Read-write access to the quaternion x component */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the quaternion x component */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the quaternion y component */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the quaternion y component */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the quaternion z component */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the quaternion z component */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to quaternion's Euler roll angle component */ - double roll() {return fuse_core::getRoll(w(), x(), y(), z());} + double roll() + { + return fuse_core::getRoll(w(), x(), y(), z()); + } /** * @brief Read-only access to quaternion's Euler pitch angle component */ - double pitch() {return fuse_core::getPitch(w(), x(), y(), z());} + double pitch() + { + return fuse_core::getPitch(w(), x(), y(), z()); + } /** * @brief Read-only access to quaternion's Euler yaw angle component */ - double yaw() {return fuse_core::getYaw(w(), x(), y(), z());} + double yaw() + { + return fuse_core::getYaw(w(), x(), y(), z()); + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Returns the number of elements of the local parameterization space. @@ -367,7 +434,10 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * 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;} + size_t localSize() const override + { + return 3u; + } /** * @brief Provides a Ceres local parameterization for the quaternion @@ -375,7 +445,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @return A pointer to a local parameterization object that indicates how to "add" increments to * the quaternion */ - fuse_core::LocalParameterization * localParameterization() const override; + fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -383,7 +453,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * * @return A pointer to a manifold object that indicates how to "add" increments to the quaternion */ - fuse_core::Manifold * manifold() const override; + fuse_core::Manifold* manifold() const override; #endif private: @@ -397,11 +467,11 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index 869d29943..623af71ac 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -80,39 +79,54 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DFixedLandmark(const uint64_t & landmark_id); + explicit Point2DFixedLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Specifies if the value of the variable should not be changed during optimization @@ -125,13 +139,16 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -140,11 +157,11 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + archive& boost::serialization::base_object>(*this); + archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 9f6673654..292d4b372 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -80,39 +79,54 @@ class Point2DLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DLandmark(const uint64_t & landmark_id); + explicit Point2DLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -120,13 +134,16 @@ class Point2DLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -135,11 +152,11 @@ class Point2DLandmark : public FixedSizeVariable<2> * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + archive& boost::serialization::base_object>(*this); + archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index 0d78b1959..699eb95fa 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -81,49 +80,70 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DFixedLandmark(const uint64_t & landmark_id); + explicit Point3DFixedLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Specifies if the value of the variable should not be changed during optimization @@ -136,13 +156,16 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -151,11 +174,11 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + archive& boost::serialization::base_object>(*this); + archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index 9b184e699..4cd048e0e 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_variables { /** @@ -84,49 +83,70 @@ class Point3DLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DLandmark(const uint64_t & landmark_id); + explicit Point3DLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -134,13 +154,16 @@ class Point3DLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -149,11 +172,11 @@ class Point3DLandmark : public FixedSizeVariable<3> * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + archive& boost::serialization::base_object>(*this); + archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index 38496d206..af349ce34 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -88,36 +88,46 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit Position2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -125,7 +135,10 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -139,11 +152,11 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index b2d22e5b8..0e7bfd09a 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -88,46 +88,62 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Position3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +151,10 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +168,11 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/stamped.hpp b/fuse_variables/include/fuse_variables/stamped.hpp index 5fcf2a7f1..4f2da197d 100644 --- a/fuse_variables/include/fuse_variables/stamped.hpp +++ b/fuse_variables/include/fuse_variables/stamped.hpp @@ -67,12 +67,10 @@ class Stamped /** * @brief Constructor */ - explicit Stamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : device_id_(device_id), - stamp_(stamp) - {} + explicit Stamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : device_id_(device_id), stamp_(stamp) + { + } /** * @brief Destructor @@ -82,16 +80,22 @@ class Stamped /** * @brief Read-only access to the associated timestamp. */ - const rclcpp::Time & stamp() const {return stamp_;} + const rclcpp::Time& stamp() const + { + return stamp_; + } /** * @brief Read-only access to the associated device ID. */ - const fuse_core::UUID & deviceId() const {return device_id_;} + const fuse_core::UUID& deviceId() const + { + return device_id_; + } private: - fuse_core::UUID device_id_; //!< The UUID associated with this specific device or hardware - rclcpp::Time stamp_{0, 0, RCL_ROS_TIME}; //!< The timestamp associated with this variable + fuse_core::UUID device_id_; //!< The UUID associated with this specific device or hardware + rclcpp::Time stamp_{ 0, 0, RCL_ROS_TIME }; //!< The timestamp associated with this variable //!< instance // Allow Boost Serialization access to private methods @@ -104,11 +108,11 @@ class Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & device_id_; - archive & stamp_; + archive& device_id_; + archive& stamp_; } }; @@ -133,8 +137,8 @@ class Stamped * @param[in] interfaces The node interfaces used to load parameters * @return A device UUID */ -fuse_core::UUID loadDeviceId( - fuse_core::node_interfaces::NodeInterfaces interfaces); +fuse_core::UUID +loadDeviceId(fuse_core::node_interfaces::NodeInterfaces interfaces); } // namespace fuse_variables 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 2445fdbc4..7f8ad356d 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_angular_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_angular_2d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 91aa62a2b..1050bb7c1 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -86,26 +86,30 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular velocity. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the angular velocity. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -113,7 +117,10 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -127,11 +134,11 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 199deab25..0a5dfe2d8 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_angular_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_angular_3d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index 312e58d59..000207d9f 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -88,46 +88,62 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular velocity. */ - double & roll() {return data_[ROLL];} + double& roll() + { + return data_[ROLL]; + } /** * @brief Read-only access to the roll (X-axis) angular velocity. */ - const double & roll() const {return data_[ROLL];} + const double& roll() const + { + return data_[ROLL]; + } /** * @brief Read-write access to the pitch (Y-axis) angular velocity. */ - double & pitch() {return data_[PITCH];} + double& pitch() + { + return data_[PITCH]; + } /** * @brief Read-only access to the pitch (Y-axis) angular velocity. */ - const double & pitch() const {return data_[PITCH];} + const double& pitch() const + { + return data_[PITCH]; + } /** * @brief Read-write access to the yaw (Z-axis) angular velocity. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the yaw (Z-axis) angular velocity. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +151,10 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +168,11 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 5641b6f59..04aaf0413 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_linear_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_linear_2d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index 19ab2de72..9565bf33f 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -88,36 +88,46 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit VelocityLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear velocity. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear velocity. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear velocity. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -125,7 +135,10 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -139,11 +152,11 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; 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 d091e4ef6..cc8048e16 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_linear_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_linear_3d_stamped.hpp instead #include diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index a86c0e1cb..9c0c8b35b 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -89,46 +89,62 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * robots or devices * */ - explicit VelocityLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_ids = fuse_core::uuid::NIL); + explicit VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_ids = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear velocity. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear velocity. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear velocity. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis linear velocity. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis linear velocity. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -136,7 +152,10 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -150,11 +169,11 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp index 55d0b54c0..2cdb40ed6 100644 --- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationAngular2DStamped::AccelerationAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationAngular2DStamped::AccelerationAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationAngular2DStamped::print(std::ostream & stream) const +void AccelerationAngular2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp index 1e6f4ac9b..9e4f35401 100644 --- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationAngular3DStamped::AccelerationAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationAngular3DStamped::AccelerationAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationAngular3DStamped::print(std::ostream & stream) const +void AccelerationAngular3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp index 73a0ddad8..47d2c2ca2 100644 --- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationLinear2DStamped::AccelerationLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationLinear2DStamped::AccelerationLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationLinear2DStamped::print(std::ostream & stream) const +void AccelerationLinear2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp index df924541a..5018a0742 100644 --- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationLinear3DStamped::AccelerationLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationLinear3DStamped::AccelerationLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationLinear3DStamped::print(std::ostream & stream) const +void AccelerationLinear3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 3897d6f97..7b2f2d7a0 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -46,15 +46,12 @@ namespace fuse_variables { -Orientation2DStamped::Orientation2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Orientation2DStamped::Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Orientation2DStamped::print(std::ostream & stream) const +void Orientation2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" @@ -65,13 +62,13 @@ void Orientation2DStamped::print(std::ostream & stream) const << " - yaw: " << yaw() << "\n"; } -fuse_core::LocalParameterization * Orientation2DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() const { return new Orientation2DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold * Orientation2DStamped::manifold() const +fuse_core::Manifold* Orientation2DStamped::manifold() const { return new Orientation2DManifold(); } diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index 4eb816a95..b72f73a19 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -46,15 +46,12 @@ namespace fuse_variables { -Orientation3DStamped::Orientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<4>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Orientation3DStamped::Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<4>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Orientation3DStamped::print(std::ostream & stream) const +void Orientation3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" @@ -68,13 +65,13 @@ void Orientation3DStamped::print(std::ostream & stream) const << " - z: " << z() << "\n"; } -fuse_core::LocalParameterization * Orientation3DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation3DStamped::localParameterization() const { return new Orientation3DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold * Orientation3DStamped::manifold() const +fuse_core::Manifold* Orientation3DStamped::manifold() const { return new Orientation3DManifold(); } diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index 48892c861..88886eb6f 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point2DFixedLandmark::print(std::ostream & stream) const +void Point2DFixedLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index cbbeaedb1..aede91f22 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point2DLandmark::Point2DLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point2DLandmark::print(std::ostream & stream) const +void Point2DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 52ae39680..47f553761 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point3DFixedLandmark::print(std::ostream & stream) const +void Point3DFixedLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 20e9ba840..0367b7920 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point3DLandmark::Point3DLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point3DLandmark::print(std::ostream & stream) const +void Point3DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp index 9f5f7130e..3e8675a4c 100644 --- a/fuse_variables/src/position_2d_stamped.cpp +++ b/fuse_variables/src/position_2d_stamped.cpp @@ -44,13 +44,12 @@ namespace fuse_variables { -Position2DStamped::Position2DStamped(const rclcpp::Time & stamp, const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Position2DStamped::Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Position2DStamped::print(std::ostream & stream) const +void Position2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp index ae2689972..26e23aa42 100644 --- a/fuse_variables/src/position_3d_stamped.cpp +++ b/fuse_variables/src/position_3d_stamped.cpp @@ -44,13 +44,12 @@ namespace fuse_variables { -Position3DStamped::Position3DStamped(const rclcpp::Time & stamp, const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Position3DStamped::Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Position3DStamped::print(std::ostream & stream) const +void Position3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/stamped.cpp b/fuse_variables/src/stamped.cpp index 87d5cd1a4..b5f8e4dd3 100644 --- a/fuse_variables/src/stamped.cpp +++ b/fuse_variables/src/stamped.cpp @@ -41,18 +41,20 @@ namespace fuse_variables { -fuse_core::UUID loadDeviceId( - fuse_core::node_interfaces::NodeInterfaces interfaces) +fuse_core::UUID +loadDeviceId(fuse_core::node_interfaces::NodeInterfaces interfaces) { std::string device_str; device_str = fuse_core::getParam(interfaces, "device_id", std::string()); - if (!device_str.empty()) { + if (!device_str.empty()) + { return fuse_core::uuid::from_string(device_str); } device_str = fuse_core::getParam(interfaces, "device_name", std::string()); - if (!device_str.empty()) { + if (!device_str.empty()) + { return fuse_core::uuid::generate(device_str); } diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp index 9f91b35c7..d3a0a1f4f 100644 --- a/fuse_variables/src/velocity_angular_2d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityAngular2DStamped::VelocityAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityAngular2DStamped::VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityAngular2DStamped::print(std::ostream & stream) const +void VelocityAngular2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp index 743aed502..25e8d3538 100644 --- a/fuse_variables/src/velocity_angular_3d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityAngular3DStamped::VelocityAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityAngular3DStamped::VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityAngular3DStamped::print(std::ostream & stream) const +void VelocityAngular3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp index ae8fc35af..d6db6c014 100644 --- a/fuse_variables/src/velocity_linear_2d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityLinear2DStamped::VelocityLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityLinear2DStamped::VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityLinear2DStamped::print(std::ostream & stream) const +void VelocityLinear2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp index e919b3380..1bb1d42d0 100644 --- a/fuse_variables/src/velocity_linear_3d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityLinear3DStamped::VelocityLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityLinear3DStamped::VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityLinear3DStamped::print(std::ostream & stream) const +void VelocityLinear3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index c6f527ee8..f595a98f8 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationAngular2DStamped; - TEST(AccelerationAngular2DStamped, Type) { AccelerationAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationAngular2DStamped, UUID) AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular2DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular2DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(AccelerationAngular2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular2DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationAngular2DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationAngular2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + AccelerationAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationAngular2DStamped, Stamped) struct CostFunctor { - 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(2.7); return true; @@ -117,23 +115,18 @@ struct CostFunctor TEST(AccelerationAngular2DStamped, Optimization) { // Create a AccelerationAngular2DStamped - AccelerationAngular2DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular2DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -147,8 +140,7 @@ TEST(AccelerationAngular2DStamped, Optimization) TEST(AccelerationAngular2DStamped, Serialization) { // Create a AccelerationAngular2DStamped - AccelerationAngular2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index 39d416fc9..3f6a40061 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationAngular3DStamped; - TEST(AccelerationAngular3DStamped, Type) { AccelerationAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationAngular3DStamped, UUID) AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular3DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular3DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(AccelerationAngular3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular3DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationAngular3DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationAngular3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + AccelerationAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationAngular3DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -119,20 +117,18 @@ struct CostFunctor TEST(AccelerationAngular3DStamped, Optimization) { // Create a AccelerationAngular3DStamped - AccelerationAngular3DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular3DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.roll() = 1.5; acceleration.pitch() = -3.0; acceleration.yaw() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -150,8 +146,7 @@ TEST(AccelerationAngular3DStamped, Optimization) TEST(AccelerationAngular3DStamped, Serialization) { // Create a AccelerationAngular3DStamped - AccelerationAngular3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.roll() = 1.5; expected.pitch() = -3.0; expected.yaw() = 14.0; diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index a1b4212e8..849a273fd 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationLinear2DStamped; - TEST(AccelerationLinear2DStamped, Type) { AccelerationLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationLinear2DStamped, UUID) AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear2DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear2DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(AccelerationLinear2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear2DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationLinear2DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationLinear2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + AccelerationLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationLinear2DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -118,19 +116,17 @@ struct CostFunctor TEST(AccelerationLinear2DStamped, Optimization) { // Create a AccelerationLinear2DStamped - AccelerationLinear2DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear2DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.x() = 1.5; acceleration.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -147,8 +143,7 @@ TEST(AccelerationLinear2DStamped, Optimization) TEST(AccelerationLinear2DStamped, Serialization) { // Create a AccelerationLinear2DStamped - AccelerationLinear2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index 83ac4093c..a67b3e8b5 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationLinear3DStamped; - TEST(AccelerationLinear3DStamped, Type) { AccelerationLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationLinear3DStamped, UUID) AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear3DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear3DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(AccelerationLinear3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear3DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationLinear3DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationLinear3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + AccelerationLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationLinear3DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -119,20 +117,18 @@ struct CostFunctor TEST(AccelerationLinear3DStamped, Optimization) { // Create a AccelerationLinear3DStamped - AccelerationLinear3DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear3DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.x() = 1.5; acceleration.y() = -3.0; acceleration.z() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -150,8 +146,7 @@ TEST(AccelerationLinear3DStamped, Optimization) TEST(AccelerationLinear3DStamped, Serialization) { // Create a AccelerationLinear3DStamped - AccelerationLinear3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 14.0; diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index 2ea971938..239f702d3 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -44,12 +44,14 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> public: FUSE_VARIABLE_DEFINITIONS(TestVariable) - TestVariable() - : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) - {} + TestVariable() : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) + { + } virtual ~TestVariable() = default; - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } private: // Allow Boost Serialization access to private methods @@ -62,19 +64,18 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); } }; - TEST(FixedSizeVariable, Size) { // Verify the expected size is returned TestVariable variable; - EXPECT_EQ(2u, variable.size()); // base class interface + EXPECT_EQ(2u, variable.size()); // base class interface EXPECT_EQ(2u, TestVariable::SIZE); // static member variable } @@ -84,7 +85,7 @@ TEST(FixedSizeVariable, Data) TestVariable variable; EXPECT_NO_THROW(variable.data()[0] = 1.0); EXPECT_NO_THROW(variable.data()[1] = 2.0); - const TestVariable & const_variable = variable; + const TestVariable& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.data()[0] == 1.0); EXPECT_NO_THROW(success = success && const_variable.data()[1] == 2.0); @@ -99,7 +100,7 @@ TEST(FixedSizeVariable, Array) EXPECT_NO_THROW(variable.array().at(1) = 2.0); EXPECT_NO_THROW(variable.array().front() = 3.0); EXPECT_NO_THROW(variable.array().back() = 4.0); - const TestVariable & const_variable = variable; + const TestVariable& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.array()[0] == 3.0); EXPECT_NO_THROW(success = success && const_variable.array().at(1) == 4.0); diff --git a/fuse_variables/test/test_load_device_id.cpp b/fuse_variables/test/test_load_device_id.cpp index 4b8ab693d..64c084248 100644 --- a/fuse_variables/test/test_load_device_id.cpp +++ b/fuse_variables/test/test_load_device_id.cpp @@ -47,16 +47,14 @@ class TestLoadDeviceId : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -73,84 +71,48 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) auto node = rclcpp::Node::make_shared("id1_node"); node->declare_parameter("device_id", std::string("01234567-89AB-CDEF-0123-456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id2_node"); node->declare_parameter("device_id", std::string("01234567-89ab-cdef-0123-456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id3_node"); node->declare_parameter("device_id", std::string("0123456789ABCDEF0123456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id4_node"); node->declare_parameter("device_id", std::string("0123456789abcdef0123456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id5_node"); node->declare_parameter("device_id", std::string("{01234567-89ab-cdef-0123-456789abcdef}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id6_node"); node->declare_parameter("device_id", std::string("{01234567-89AB-CDEF-0123-456789ABCDEF}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { @@ -162,14 +124,8 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) auto node = rclcpp::Node::make_shared("name_node"); node->declare_parameter("device_name", std::string("Test")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x5B, 0x23, 0x43, 0x6D, - 0x8E, 0x7C, - 0x51, 0xCF, - 0x81, 0x62, - 0x5C, 0xD5, 0xFD, 0x37, 0x9E, 0xCF - }; + fuse_core::UUID expected = { 0x5B, 0x23, 0x43, 0x6D, 0x8E, 0x7C, 0x51, 0xCF, + 0x81, 0x62, 0x5C, 0xD5, 0xFD, 0x37, 0x9E, 0xCF }; EXPECT_EQ(expected, actual); } { @@ -181,9 +137,8 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index 8e17d43bc..a20ff82f0 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -64,10 +64,8 @@ TEST(Orientation2DStamped, UUID) Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Orientation2DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Orientation2DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -83,18 +81,16 @@ TEST(Orientation2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Orientation2DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Orientation2DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Orientation2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Orientation2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + Orientation2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -108,8 +104,8 @@ TEST(Orientation2DStamped, Stamped) struct Orientation2DPlus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + 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]); return true; @@ -118,8 +114,8 @@ struct Orientation2DPlus struct Orientation2DMinus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; @@ -127,7 +123,7 @@ struct Orientation2DMinus }; using Orientation2DLocalParameterization = - fuse_core::AutoDiffLocalParameterization; + fuse_core::AutoDiffLocalParameterization; TEST(Orientation2DStamped, Plus) { @@ -135,9 +131,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); @@ -146,9 +142,9 @@ 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); @@ -163,13 +159,14 @@ TEST(Orientation2DStamped, PlusJacobian) auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + 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 = parameterization->ComputeJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.ComputeJacobian(x, expected); EXPECT_TRUE(success); @@ -185,9 +182,9 @@ TEST(Orientation2DStamped, Minus) // Simple test { - double x1[1] = {1.0}; - double x2[1] = {1.5}; - double actual[1] = {0.0}; + double x1[1] = { 1.0 }; + double x2[1] = { 1.5 }; + double actual[1] = { 0.0 }; bool success = parameterization->Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -196,9 +193,9 @@ TEST(Orientation2DStamped, Minus) // Check roll-over { - double x1[1] = {2.0}; - double x2[1] = {5 - 2 * M_PI}; - double actual[1] = {0.0}; + double x1[1] = { 2.0 }; + double x2[1] = { 5 - 2 * M_PI }; + double actual[1] = { 0.0 }; bool success = parameterization->Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -211,13 +208,14 @@ TEST(Orientation2DStamped, MinusJacobian) auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + 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 = parameterization->ComputeMinusJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.ComputeMinusJacobian(x, expected); EXPECT_TRUE(success); @@ -229,9 +227,12 @@ TEST(Orientation2DStamped, MinusJacobian) struct CostFunctor { - 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; @@ -241,28 +242,20 @@ struct CostFunctor TEST(Orientation2DStamped, Optimization) { // Create a Orientation2DStamped - Orientation2DStamped orientation( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Orientation2DStamped orientation(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); orientation.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // 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; + std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -278,8 +271,7 @@ TEST(Orientation2DStamped, Optimization) TEST(Orientation2DStamped, Serialization) { // Create a Orientation2DStamped - Orientation2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Orientation2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive @@ -307,15 +299,15 @@ TEST(Orientation2DStamped, Serialization) struct Orientation2DFunctor { - template - bool Plus(const T * x, const T * delta, T * x_plus_delta) const + 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 + 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; @@ -330,9 +322,9 @@ TEST(Orientation2DStamped, ManifoldPlus) // 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 = manifold->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -341,9 +333,9 @@ TEST(Orientation2DStamped, ManifoldPlus) // 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 = manifold->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -358,13 +350,14 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation2DManifold(); - auto test_values = std::vector {-2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + 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}; + double expected[1] = { 0.0 }; reference.PlusJacobian(x, expected); EXPECT_TRUE(success); @@ -380,9 +373,9 @@ TEST(Orientation2DStamped, ManifoldMinus) // Simple test { - double x1[1] = {1.0}; - double x2[1] = {1.5}; - double actual[1] = {0.0}; + 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); @@ -391,9 +384,9 @@ TEST(Orientation2DStamped, ManifoldMinus) // Check roll-over { - double x1[1] = {2.0}; - double x2[1] = {5 - 2 * M_PI}; - double actual[1] = {0.0}; + 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); @@ -406,13 +399,14 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation2DManifold(); - auto test_values = std::vector {-2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + 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}; + double expected[1] = { 0.0 }; reference.MinusJacobian(x, expected); EXPECT_TRUE(success); diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 10e04d0b8..c50926b28 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -110,7 +110,7 @@ TEST(Orientation3DStamped, UUID) } } -template +template inline static void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; @@ -121,8 +121,8 @@ inline static void QuaternionInverse(const T in[4], T out[4]) struct Orientation3DPlus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -133,8 +133,8 @@ struct Orientation3DPlus struct Orientation3DMinus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { T x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -146,15 +146,15 @@ struct Orientation3DMinus }; using Orientation3DLocalParameterization = - fuse_core::AutoDiffLocalParameterization; + fuse_core::AutoDiffLocalParameterization; TEST(Orientation3DStamped, Plus) { auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); - 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}; + 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 = parameterization->Plus(x, delta, result); EXPECT_TRUE(success); @@ -170,9 +170,9 @@ TEST(Orientation3DStamped, Minus) { auto parameterization = Orientation3DStamped(rclcpp::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}; + 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 }; bool success = parameterization->Minus(x1, x2, result); EXPECT_TRUE(success); @@ -188,39 +188,35 @@ TEST(Orientation3DStamped, PlusJacobian) auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); - 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) { + 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}; + double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ - actual << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ bool success = parameterization->ComputeJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + expected << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ reference.ComputeJacobian(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"; + 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"; } } } @@ -233,39 +229,35 @@ TEST(Orientation3DStamped, MinusJacobian) auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); - 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) { + 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}; + double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ - actual << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ bool success = parameterization->ComputeMinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + expected << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ reference.ComputeMinusJacobian(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"; + 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"; } } } @@ -275,8 +267,8 @@ TEST(Orientation3DStamped, MinusJacobian) TEST(Orientation3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Orientation3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + Orientation3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -290,7 +282,7 @@ TEST(Orientation3DStamped, Stamped) struct QuaternionCostFunction { - explicit QuaternionCostFunction(double * observation) + explicit QuaternionCostFunction(double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -298,24 +290,12 @@ struct QuaternionCostFunction observation_[3] = observation[3]; } - template - bool operator()(const T * quaternion, T * residual) const + template + bool operator()(const T* quaternion, T* residual) const { - T inverse_quaternion[4] = - { - quaternion[0], - -quaternion[1], - -quaternion[2], - -quaternion[3] - }; + 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 observation[4] = { T(observation_[0]), T(observation_[1]), T(observation_[2]), T(observation_[3]) }; T output[4]; @@ -342,25 +322,18 @@ TEST(Orientation3DStamped, Optimization) orientation.z() = 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)); + 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; #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; + std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -443,16 +416,16 @@ TEST(Orientation3DStamped, Serialization) struct Orientation3DFunctor { - template - bool Plus(const T * x, const T * delta, T * x_plus_delta) const + 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 + template + bool Minus(const T* y, const T* x, T* y_minus_x) const { T x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -469,9 +442,9 @@ TEST(Orientation3DStamped, ManifoldPlus) { auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); - double x[4] = {0.842614977, 0.2, 0.3, 0.4}; - double delta[3] = {0.15, -0.2, 0.433012702}; - double result[4] = {0.0, 0.0, 0.0, 0.0}; + 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); @@ -488,39 +461,35 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); - for (double qx = -0.5; qx < 0.5; qx += 0.1) { - for (double qy = -0.5; qy < 0.5; qy += 0.1) { - for (double qz = -0.5; qz < 0.5; qz += 0.1) { + 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}; + double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ - actual << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ bool success = manifold->PlusJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + expected << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ reference.PlusJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << "\n" - << "Difference is:\n" << (expected - actual).format(clean) - << "\n"; + 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"; } } } @@ -530,9 +499,9 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) 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}; + double x1[4] = { 0.842614977, 0.2, 0.3, 0.4 }; + double x2[4] = { 0.745561, 0.360184, 0.194124, 0.526043 }; + double result[3] = { 0.0, 0.0, 0.0 }; auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); bool success = manifold->Minus(x2, x1, result); @@ -550,37 +519,35 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); - for (double qx = -0.5; qx < 0.5; qx += 0.1) { - for (double qy = -0.5; qy < 0.5; qy += 0.1) { - for (double qz = -0.5; qz < 0.5; qz += 0.1) { + 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}; + double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ - actual << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ bool success = manifold->MinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + expected << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ reference.MinusJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << "\n" - << "Difference is:\n" << (expected - actual).format(clean) - << "\n"; + 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"; } } } diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 9aa369adf..45b13ffd7 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point2DFixedLandmark; - TEST(Point2DFixedLandmark, Type) { Point2DFixedLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point2DFixedLandmark, UUID) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -90,16 +92,16 @@ TEST(Point2DFixedLandmark, Optimization) position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); - if (position.holdConstant()) { + if (position.holdConstant()) + { problem.SetParameterBlockConstant(position.data()); } diff --git a/fuse_variables/test/test_point_2d_landmark.cpp b/fuse_variables/test/test_point_2d_landmark.cpp index 8ca6d5b99..0615470dd 100644 --- a/fuse_variables/test/test_point_2d_landmark.cpp +++ b/fuse_variables/test/test_point_2d_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point2DLandmark; - TEST(Point2DLandmark, Type) { Point2DLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point2DLandmark, UUID) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -90,13 +92,12 @@ TEST(Point2DLandmark, Optimization) position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_point_3d_fixed_landmark.cpp b/fuse_variables/test/test_point_3d_fixed_landmark.cpp index 3376c0306..5bdf54a46 100644 --- a/fuse_variables/test/test_point_3d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_3d_fixed_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point3DFixedLandmark; - TEST(Point3DFixedLandmark, Type) { Point3DFixedLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point3DFixedLandmark, UUID) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -92,16 +94,16 @@ TEST(Point3DFixedLandmark, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); - if (position.holdConstant()) { + if (position.holdConstant()) + { problem.SetParameterBlockConstant(position.data()); } diff --git a/fuse_variables/test/test_point_3d_landmark.cpp b/fuse_variables/test/test_point_3d_landmark.cpp index 287bb6d51..055b25331 100644 --- a/fuse_variables/test/test_point_3d_landmark.cpp +++ b/fuse_variables/test/test_point_3d_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point3DLandmark; - TEST(Point3DLandmark, Type) { Point3DLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point3DLandmark, UUID) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -92,13 +94,12 @@ TEST(Point3DLandmark, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index 584ea8ef4..ce35789db 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::Position2DStamped; - TEST(Position2DStamped, Type) { Position2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(Position2DStamped, UUID) Position2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Position2DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Position2DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(Position2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Position2DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Position2DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Position2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Position2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Position2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Position2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + Position2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(Position2DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -118,19 +116,17 @@ struct CostFunctor TEST(Position2DStamped, Optimization) { // Create a Position2DStamped - Position2DStamped position( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Position2DStamped position(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); position.x() = 1.5; position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -147,8 +143,7 @@ TEST(Position2DStamped, Optimization) TEST(Position2DStamped, Serialization) { // Create a Position2DStamped - Position2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Position2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index 112943f2a..c70746b53 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::Position3DStamped; - TEST(Position3DStamped, Type) { Position3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -106,8 +105,8 @@ TEST(Position3DStamped, UUID) TEST(Position3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Position3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + Position3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -121,9 +120,12 @@ TEST(Position3DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -141,18 +143,14 @@ TEST(Position3DStamped, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -168,8 +166,7 @@ TEST(Position3DStamped, Optimization) TEST(Position3DStamped, Serialization) { // Create a Position3DStamped - Position3DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + Position3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 0.8; diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index bae0a296a..029af9778 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityAngular2DStamped; - TEST(VelocityAngular2DStamped, Type) { VelocityAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityAngular2DStamped, UUID) VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular2DStamped variable3( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityAngular2DStamped variable4( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(VelocityAngular2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular2DStamped variable1( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - VelocityAngular2DStamped variable2( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityAngular2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + VelocityAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(VelocityAngular2DStamped, Stamped) struct CostFunctor { - 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(2.7); return true; @@ -117,18 +115,16 @@ struct CostFunctor TEST(VelocityAngular2DStamped, Optimization) { // Create a VelocityAngular2DStamped - VelocityAngular2DStamped velocity(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular2DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -144,8 +140,7 @@ TEST(VelocityAngular2DStamped, Optimization) TEST(VelocityAngular2DStamped, Serialization) { // Create a VelocityAngular2DStamped - VelocityAngular2DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index d1378aad1..5d269cb39 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityAngular3DStamped; - TEST(VelocityAngular3DStamped, Type) { VelocityAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityAngular3DStamped, UUID) VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); - VelocityAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,19 +77,16 @@ TEST(VelocityAngular3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("8d8")); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("r4-p17")); + VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityAngular3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + VelocityAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -106,9 +100,12 @@ TEST(VelocityAngular3DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -120,25 +117,20 @@ struct CostFunctor TEST(VelocityAngular3DStamped, Optimization) { // Create a VelocityAngular3DStamped - VelocityAngular3DStamped velocity(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular3DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.roll() = 1.5; velocity.pitch() = -3.0; velocity.yaw() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -154,8 +146,7 @@ TEST(VelocityAngular3DStamped, Optimization) TEST(VelocityAngular3DStamped, Serialization) { // Create a VelocityAngular3DStamped - VelocityAngular3DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.roll() = 1.5; expected.pitch() = -3.0; expected.yaw() = 14.0; diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index 914b13d32..ea8aef29e 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityLinear2DStamped; - TEST(VelocityLinear2DStamped, Type) { VelocityLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityLinear2DStamped, UUID) VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); - VelocityLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ TEST(VelocityLinear2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("r2d2")); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("bb8")); + VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityLinear2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + VelocityLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(VelocityLinear2DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -118,24 +116,19 @@ struct CostFunctor TEST(VelocityLinear2DStamped, Optimization) { // Create a VelocityLinear2DStamped - VelocityLinear2DStamped velocity( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear2DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.x() = 1.5; velocity.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -150,8 +143,7 @@ TEST(VelocityLinear2DStamped, Optimization) TEST(VelocityLinear2DStamped, Serialization) { // Create a VelocityLinear2DStamped - VelocityLinear2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 59e6909d0..7bcca429d 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityLinear3DStamped; - TEST(VelocityLinear3DStamped, Type) { VelocityLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityLinear3DStamped, UUID) VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); - VelocityLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,19 +77,16 @@ TEST(VelocityLinear3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("8d8")); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("r4-p17")); + VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityLinear3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr base = + VelocityLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -106,9 +100,12 @@ TEST(VelocityLinear3DStamped, Stamped) struct CostFunctor { - 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); residual[1] = x[1] + T(8.0); @@ -120,25 +117,20 @@ struct CostFunctor TEST(VelocityLinear3DStamped, Optimization) { // Create a VelocityLinear3DStamped - VelocityLinear3DStamped velocity( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear3DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.x() = 1.5; velocity.y() = -3.0; velocity.z() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -154,8 +146,7 @@ TEST(VelocityLinear3DStamped, Optimization) TEST(VelocityLinear3DStamped, Serialization) { // Create a VelocityLinear3DStamped - VelocityLinear3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 14.0; diff --git a/fuse_viz/include/fuse_viz/conversions.hpp b/fuse_viz/include/fuse_viz/conversions.hpp index 18428a628..63897a506 100644 --- a/fuse_viz/include/fuse_viz/conversions.hpp +++ b/fuse_viz/include/fuse_viz/conversions.hpp @@ -53,7 +53,6 @@ #include #include - namespace tf2 { @@ -62,8 +61,8 @@ namespace tf2 * @param[in] covariance 3x3 covariance matrix. * @param[out] msg 6x6 covariance message, which is stored as a plain array with 36 elements. */ -template -inline void toMsg(const Eigen::MatrixBase & covariance, std::array & msg) +template +inline void toMsg(const Eigen::MatrixBase& covariance, std::array& msg) { using Scalar = typename Derived::Scalar; using Matrix6 = Eigen::Matrix; @@ -83,41 +82,39 @@ inline void toMsg(const Eigen::MatrixBase & covariance, std::array(position.x()), static_cast(position.y()), - static_cast(position.z())}; + return { static_cast(position.x()), static_cast(position.y()), static_cast(position.z()) }; } -inline Ogre::Quaternion toOgre(const tf2::Quaternion & orientation) +inline Ogre::Quaternion toOgre(const tf2::Quaternion& orientation) { - return {static_cast(orientation.w()), static_cast(orientation.x()), // NOLINT - static_cast(orientation.y()), static_cast(orientation.z())}; + return { static_cast(orientation.w()), static_cast(orientation.x()), // NOLINT + static_cast(orientation.y()), static_cast(orientation.z()) }; } -inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped & position) +inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped& position) { - return {static_cast(position.x()), static_cast(position.y()), 0}; + return { static_cast(position.x()), static_cast(position.y()), 0 }; } -inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped & orientation) +inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped& orientation) { return toOgre(toTF(orientation)); } @@ -127,31 +124,28 @@ inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped & orie namespace { -inline tf2::Transform getPose( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +inline tf2::Transform getPose(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { return tf2::Transform(toTF(orientation), toTF(position)); } -inline tf2::Transform getPose( - const fuse_core::Graph & graph, const fuse_core::UUID & position_uuid, - const fuse_core::UUID & orientation_uuid) +inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UUID& position_uuid, + const fuse_core::UUID& orientation_uuid) { - const auto position = - dynamic_cast(&graph.getVariable(position_uuid)); - if (!position) { - throw std::runtime_error( - "Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + - " from graph as fuse_variables::Position2DStamped."); + const auto position = dynamic_cast(&graph.getVariable(position_uuid)); + if (!position) + { + throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + + " from graph as fuse_variables::Position2DStamped."); } - const auto orientation = dynamic_cast( - &graph.getVariable(orientation_uuid)); - if (!orientation) { - throw std::runtime_error( - "Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + - " from graph as fuse_variables::Orientation2DStamped."); + const auto orientation = + dynamic_cast(&graph.getVariable(orientation_uuid)); + if (!orientation) + { + throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + + " from graph as fuse_variables::Orientation2DStamped."); } return getPose(*position, *orientation); diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp index d3b9130c5..120a511df 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp @@ -50,7 +50,6 @@ #include #include - namespace Ogre { @@ -87,10 +86,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty RGB, }; - MappedCovarianceProperty( - const QString & name = "Covariance", bool default_value = false, - const QString & description = QString(), rviz_common::properties::Property * parent = 0, - const char * changed_slot = 0, QObject * receiver = 0); + MappedCovarianceProperty(const QString& name = "Covariance", bool default_value = false, + const QString& description = QString(), rviz_common::properties::Property* parent = 0, + const char* changed_slot = 0, QObject* receiver = 0); virtual ~MappedCovarianceProperty(); @@ -98,10 +96,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty bool getOrientationBool(); // Methods to manage the unordered map of Covariance Visuals - MappedCovarianceVisualPtr createAndInsertVisual( - const std::string & key, Ogre::SceneManager * scene_manager, - Ogre::SceneNode * parent_node); - void eraseVisual(const std::string & key); + MappedCovarianceVisualPtr createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node); + void eraseVisual(const std::string& key); void clearVisual(); size_t sizeVisual(); @@ -114,23 +111,23 @@ private Q_SLOTS: void updateColorStyleChoice(); private: - void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr & visual); - void updateOrientationFrame(const MappedCovarianceVisualPtr & visual); - void updateVisibility(const MappedCovarianceVisualPtr & visual); + void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual); + void updateOrientationFrame(const MappedCovarianceVisualPtr& visual); + void updateVisibility(const MappedCovarianceVisualPtr& visual); std::unordered_map covariances_; - rviz_common::properties::BoolProperty * position_property_; - rviz_common::properties::ColorProperty * position_color_property_; - rviz_common::properties::FloatProperty * position_alpha_property_; - rviz_common::properties::FloatProperty * position_scale_property_; - rviz_common::properties::BoolProperty * orientation_property_; - rviz_common::properties::EnumProperty * orientation_frame_property_; - rviz_common::properties::EnumProperty * orientation_colorstyle_property_; - rviz_common::properties::ColorProperty * orientation_color_property_; - rviz_common::properties::FloatProperty * orientation_alpha_property_; - rviz_common::properties::FloatProperty * orientation_offset_property_; - rviz_common::properties::FloatProperty * orientation_scale_property_; + rviz_common::properties::BoolProperty* position_property_; + rviz_common::properties::ColorProperty* position_color_property_; + rviz_common::properties::FloatProperty* position_alpha_property_; + rviz_common::properties::FloatProperty* position_scale_property_; + rviz_common::properties::BoolProperty* orientation_property_; + rviz_common::properties::EnumProperty* orientation_frame_property_; + rviz_common::properties::EnumProperty* orientation_colorstyle_property_; + rviz_common::properties::ColorProperty* orientation_color_property_; + rviz_common::properties::FloatProperty* orientation_alpha_property_; + rviz_common::properties::FloatProperty* orientation_offset_property_; + rviz_common::properties::FloatProperty* orientation_scale_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp index 484e727a6..b5f4d3dc1 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp @@ -46,7 +46,6 @@ #include - namespace Ogre { class SceneManager; @@ -96,10 +95,9 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param pos_scale Scale of the position covariance * @param ori_scale Scale of the orientation covariance */ - MappedCovarianceVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, bool is_local_rotation, - bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f, - float ori_offset = 0.1f); + MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, + bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f, + float ori_offset = 0.1f); public: ~MappedCovarianceVisual() override; @@ -123,7 +121,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setPositionColor(float r, float g, float b, float a); - void setPositionColor(const Ogre::ColourValue & color); + void setPositionColor(const Ogre::ColourValue& color); /** * \brief Set the color of the orientation covariance. Values are in the range [0, 1] @@ -133,7 +131,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setOrientationColor(float r, float g, float b, float a); - void setOrientationColor(const Ogre::ColourValue & color); + void setOrientationColor(const Ogre::ColourValue& color); void setOrientationColorToRGB(float a); /** @brief Set the covariance. @@ -141,16 +139,16 @@ class MappedCovarianceVisual : public rviz_rendering::Object * This effectively changes the orientation and scale of position and orientation * covariance shapes */ - virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose); + virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose); - virtual const Ogre::Vector3 & getPositionCovarianceScale(); - virtual const Ogre::Quaternion & getPositionCovarianceOrientation(); + virtual const Ogre::Vector3& getPositionCovarianceScale(); + virtual const Ogre::Quaternion& getPositionCovarianceOrientation(); /** * \brief Get the root scene node of the position part of this covariance * @return the root scene node of the position part of this covariance */ - Ogre::SceneNode * getPositionSceneNode() + Ogre::SceneNode* getPositionSceneNode() { return position_scale_node_; } @@ -159,7 +157,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the root scene node of the orientation part of this covariance * @return the root scene node of the orientation part of this covariance */ - Ogre::SceneNode * getOrientationSceneNode() + Ogre::SceneNode* getOrientationSceneNode() { return orientation_root_node_; } @@ -168,7 +166,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the shape used to display position covariance * @return the shape used to display position covariance */ - rviz_rendering::Shape * getPositionShape() + rviz_rendering::Shape* getPositionShape() { return position_shape_; } @@ -177,12 +175,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the shape used to display orientation covariance in an especific axis * @return the shape used to display orientation covariance in an especific axis */ - rviz_rendering::Shape * getOrientationShape(ShapeIndex index); + rviz_rendering::Shape* getOrientationShape(ShapeIndex index); /** * \brief Sets user data on all ogre objects we own */ - virtual void setUserData(const Ogre::Any & data); + virtual void setUserData(const Ogre::Any& data); /** * \brief Sets visibility of this covariance @@ -204,12 +202,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object /** * \brief Sets position of the frame this covariance is attached */ - virtual void setPosition(const Ogre::Vector3 & position); + virtual void setPosition(const Ogre::Vector3& position); /** * \brief Sets orientation of the frame this covariance is attached */ - virtual void setOrientation(const Ogre::Quaternion & orientation); + virtual void setOrientation(const Ogre::Quaternion& orientation); /** * \brief Sets which frame to attach the covariance of the orientation @@ -217,22 +215,22 @@ class MappedCovarianceVisual : public rviz_rendering::Object virtual void setRotatingFrame(bool use_rotating_frame); private: - void updatePosition(const Eigen::Matrix6d & covariance); - void updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index); + void updatePosition(const Eigen::Matrix6d& covariance); + void updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index); void updateOrientationVisibility(); - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * fixed_orientation_node_ = nullptr; - Ogre::SceneNode * position_scale_node_ = nullptr; - Ogre::SceneNode * position_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* fixed_orientation_node_ = nullptr; + Ogre::SceneNode* position_scale_node_ = nullptr; + Ogre::SceneNode* position_node_ = nullptr; - Ogre::SceneNode * orientation_root_node_ = nullptr; - Ogre::SceneNode * orientation_offset_node_[kNumOriShapes]; + Ogre::SceneNode* orientation_root_node_ = nullptr; + Ogre::SceneNode* orientation_offset_node_[kNumOriShapes]; - rviz_rendering::Shape * position_shape_; //!< Ellipse used for the position - //!< covariance - rviz_rendering::Shape * orientation_shape_[kNumOriShapes]; //!< Cylinders used for the - //!< orientation covariance + rviz_rendering::Shape* position_shape_; //!< Ellipse used for the position + //!< covariance + rviz_rendering::Shape* orientation_shape_[kNumOriShapes]; //!< Cylinders used for the + //!< orientation covariance bool local_rotation_; @@ -248,14 +246,14 @@ class MappedCovarianceVisual : public rviz_rendering::Object private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - virtual void setScale(const Ogre::Vector3 &) + virtual void setScale(const Ogre::Vector3&) { } virtual void setColor(float, float, float, float) { } - virtual const Ogre::Vector3 & getPosition(); - virtual const Ogre::Quaternion & getOrientation(); + virtual const Ogre::Vector3& getPosition(); + virtual const Ogre::Quaternion& getOrientation(); // Make MappedCovarianceProperty friend class so it create MappedCovarianceVisual objects friend class MappedCovarianceProperty; diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp index 33d04ef9f..ed01eac0b 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp @@ -44,7 +44,6 @@ #include - namespace Ogre { @@ -74,18 +73,16 @@ class Pose2DStampedProperty : public rviz_common::properties::BoolProperty using Visual = Pose2DStampedVisual; using VisualPtr = std::shared_ptr; - Pose2DStampedProperty( - const QString & name = "Pose2DStamped", bool default_value = true, - const QString & description = QString(), Property * parent = NULL, - const char * changed_slot = NULL, QObject * receiver = NULL); + Pose2DStampedProperty(const QString& name = "Pose2DStamped", bool default_value = true, + const QString& description = QString(), Property* parent = NULL, + const char* changed_slot = NULL, QObject* receiver = NULL); ~Pose2DStampedProperty() override = default; - VisualPtr createAndInsertOrUpdateVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation); - void eraseVisual(const fuse_core::UUID & uuid); + VisualPtr createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation); + void eraseVisual(const fuse_core::UUID& uuid); void clearVisual(); public Q_SLOTS: @@ -99,21 +96,21 @@ private Q_SLOTS: void updateTextScale(); private: - void updateAxesAlpha(const VisualPtr & constraint); - void updateScale(const VisualPtr & constraint); - void updateShowText(const VisualPtr & constraint); - void updateSphereColorAlpha(const VisualPtr & constraint); - void updateTextScale(const VisualPtr & constraint); - void updateVisibility(const VisualPtr & constraint); + void updateAxesAlpha(const VisualPtr& constraint); + void updateScale(const VisualPtr& constraint); + void updateShowText(const VisualPtr& constraint); + void updateSphereColorAlpha(const VisualPtr& constraint); + void updateTextScale(const VisualPtr& constraint); + void updateVisibility(const VisualPtr& constraint); std::unordered_map variables_; - rviz_common::properties::ColorProperty * color_property_; - rviz_common::properties::BoolProperty * show_text_property_; - rviz_common::properties::FloatProperty * sphere_alpha_property_; - rviz_common::properties::FloatProperty * axes_alpha_property_; - rviz_common::properties::FloatProperty * scale_property_; - rviz_common::properties::FloatProperty * text_scale_property_; + rviz_common::properties::ColorProperty* color_property_; + rviz_common::properties::BoolProperty* show_text_property_; + rviz_common::properties::FloatProperty* sphere_alpha_property_; + rviz_common::properties::FloatProperty* axes_alpha_property_; + rviz_common::properties::FloatProperty* scale_property_; + rviz_common::properties::FloatProperty* text_scale_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp index dc240f7ad..67ed1bdbc 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp @@ -45,7 +45,6 @@ #include #include - namespace Ogre { @@ -89,10 +88,9 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] orientation fuse_variables::Orientation2DStamped orientation. * @param[in] visible Initial visibility. */ - Pose2DStampedVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, const bool visible = true); + Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const bool visible = true); public: ~Pose2DStampedVisual() override; @@ -102,15 +100,14 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] position 2D position stamped variable. * @param[in] orientation 2D orientation stamped variable. */ - void setPose2DStamped( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation); + void setPose2DStamped(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation); /** * @brief Get the root scene node of this variable visual. * @return the root scene node of this variable visual. */ - Ogre::SceneNode * getSceneNode() + Ogre::SceneNode* getSceneNode() { return root_node_; } @@ -118,15 +115,15 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any & data) override; + void setUserData(const Ogre::Any& data) override; void setSphereColor(const float r, const float g, const float b, const float a); void setAxesAlpha(const float alpha); - void setScale(const Ogre::Vector3 & scale) override; + void setScale(const Ogre::Vector3& scale) override; - void setTextScale(const Ogre::Vector3 & scale); + void setTextScale(const Ogre::Vector3& scale); void setTextVisible(const bool visible); @@ -140,33 +137,35 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3 & position) override; + void setPosition(const Ogre::Vector3& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion & orientation) override; + void setOrientation(const Ogre::Quaternion& orientation) override; private: - void setPose2DStamped(const Ogre::Vector3 & position, const Ogre::Quaternion & orientation); + void setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * sphere_node_ = nullptr; - Ogre::SceneNode * axes_node_ = nullptr; - Ogre::SceneNode * text_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* sphere_node_ = nullptr; + Ogre::SceneNode* axes_node_ = nullptr; + Ogre::SceneNode* text_node_ = nullptr; bool visible_; std::shared_ptr axes_; std::shared_ptr sphere_; - rviz_rendering::MovableText * text_; + rviz_rendering::MovableText* text_; private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - void setColor(float, float, float, float) override {} - const Ogre::Vector3 & getPosition() override; - const Ogre::Quaternion & getOrientation() override; + void setColor(float, float, float, float) override + { + } + const Ogre::Vector3& getPosition() override; + const Ogre::Quaternion& getOrientation() override; // Make Pose2DStampedProperty friend class so it create Pose2DStampedVisual objects friend class Pose2DStampedProperty; diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h index 0c952b1fd..d879ee071 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h @@ -35,9 +35,7 @@ #ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ #define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ -#warning \ - This header is obsolete, please include \ - fuse_viz/relative_pose_2d_stamped_constraint_property.hpp instead +#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_property.hpp instead #include diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp index 637855afc..7bfb32ebd 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp @@ -47,7 +47,6 @@ #include #include - namespace Ogre { @@ -91,22 +90,20 @@ class RelativePose2DStampedConstraintProperty : public BoolProperty using Visual = RelativePose2DStampedConstraintVisual; using VisualPtr = std::shared_ptr; - RelativePose2DStampedConstraintProperty( - const QString & name = "RelativePose2DStampedConstraint", - bool default_value = true, const QString & description = QString(), - Property * parent = NULL, const char * changed_slot = NULL, - QObject * receiver = NULL); + RelativePose2DStampedConstraintProperty(const QString& name = "RelativePose2DStampedConstraint", + bool default_value = true, const QString& description = QString(), + Property* parent = NULL, const char* changed_slot = NULL, + QObject* receiver = NULL); ~RelativePose2DStampedConstraintProperty() override = default; - VisualPtr createAndInsertVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph); - void eraseVisual(const fuse_core::UUID & uuid); + VisualPtr createAndInsertVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, + const fuse_core::Graph& graph); + void eraseVisual(const fuse_core::UUID& uuid); void clearVisual(); - void setColor(const QColor & color); + void setColor(const QColor& color); public Q_SLOTS: void updateVisibility(); @@ -124,31 +121,31 @@ private Q_SLOTS: void updateTextScale(); private: - void updateColor(const VisualPtr & constraint); - void updateErrorLineAlpha(const VisualPtr & constraint); - void updateErrorLineWidth(const VisualPtr & constraint); - void updateLossMinBrightness(const VisualPtr & constraint); - void updateRelativePoseAxesAlpha(const VisualPtr & constraint); - void updateRelativePoseAxesScale(const VisualPtr & constraint); - void updateRelativePoseLineAlpha(const VisualPtr & constraint); - void updateRelativePoseLineWidth(const VisualPtr & constraint); - void updateShowText(const VisualPtr & constraint); - void updateTextScale(const VisualPtr & constraint); - void updateVisibility(const VisualPtr & constraint); + void updateColor(const VisualPtr& constraint); + void updateErrorLineAlpha(const VisualPtr& constraint); + void updateErrorLineWidth(const VisualPtr& constraint); + void updateLossMinBrightness(const VisualPtr& constraint); + void updateRelativePoseAxesAlpha(const VisualPtr& constraint); + void updateRelativePoseAxesScale(const VisualPtr& constraint); + void updateRelativePoseLineAlpha(const VisualPtr& constraint); + void updateRelativePoseLineWidth(const VisualPtr& constraint); + void updateShowText(const VisualPtr& constraint); + void updateTextScale(const VisualPtr& constraint); + void updateVisibility(const VisualPtr& constraint); std::unordered_map constraints_; - ColorProperty * color_property_; - BoolProperty * show_text_property_; - FloatProperty * text_scale_property_; - FloatProperty * relative_pose_axes_alpha_property_; - FloatProperty * relative_pose_axes_scale_property_; - FloatProperty * relative_pose_line_alpha_property_; - FloatProperty * relative_pose_line_width_property_; - FloatProperty * error_line_alpha_property_; - FloatProperty * error_line_width_property_; - FloatProperty * loss_min_brightness_property_; - MappedCovarianceProperty * covariance_property_; + ColorProperty* color_property_; + BoolProperty* show_text_property_; + FloatProperty* text_scale_property_; + FloatProperty* relative_pose_axes_alpha_property_; + FloatProperty* relative_pose_axes_scale_property_; + FloatProperty* relative_pose_line_alpha_property_; + FloatProperty* relative_pose_line_width_property_; + FloatProperty* error_line_alpha_property_; + FloatProperty* error_line_width_property_; + FloatProperty* loss_min_brightness_property_; + MappedCovarianceProperty* covariance_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h index ea506f767..fb3f22d0f 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h @@ -35,8 +35,7 @@ #ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ #define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ -#warning \ - This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp \ +#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp \ instead #include diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp index c13785839..59a38650c 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp @@ -48,7 +48,6 @@ #include #include - namespace Ogre { @@ -109,10 +108,9 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] constraint fuse_constraints::RelativePose2DStampedConstraint constraint. * @param[in] visible Initial visibility. */ - RelativePose2DStampedConstraintVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const bool visible = true); + RelativePose2DStampedConstraintVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, + const bool visible = true); public: using CovarianceVisualPtr = MappedCovarianceProperty::MappedCovarianceVisualPtr; @@ -125,15 +123,13 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] graph fuse_core::Graph, used to retrieve the first/source and second/target * constraint variables pose. */ - void setConstraint( - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph); + void setConstraint(const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph); /** * @brief Get the root scene node of this constraint visual * @return the root scene node of this constraint visual */ - Ogre::SceneNode * getSceneNode() + Ogre::SceneNode* getSceneNode() { return root_node_; } @@ -141,7 +137,7 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any & data) override; + void setUserData(const Ogre::Any& data) override; void setRelativePoseLineWidth(const float line_width); @@ -155,9 +151,9 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object void setRelativePoseAxesAlpha(const float alpha); - void setRelativePoseAxesScale(const Ogre::Vector3 & scale); + void setRelativePoseAxesScale(const Ogre::Vector3& scale); - void setTextScale(const Ogre::Vector3 & scale); + void setTextScale(const Ogre::Vector3& scale); void setTextVisible(const bool visible); @@ -171,44 +167,44 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3 & position) override; + void setPosition(const Ogre::Vector3& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion & orientation) override; + void setOrientation(const Ogre::Quaternion& orientation) override; - const CovarianceVisualPtr & getCovariance() const + const CovarianceVisualPtr& getCovariance() const { return covariance_; } - void setCovariance(const CovarianceVisualPtr & covariance) + void setCovariance(const CovarianceVisualPtr& covariance) { covariance_ = covariance; } - const std::string & getSource() const + const std::string& getSource() const { return source_; } private: - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * relative_pose_line_node_ = nullptr; - Ogre::SceneNode * error_line_node_ = nullptr; - Ogre::SceneNode * relative_pose_axes_node_ = nullptr; - Ogre::SceneNode * text_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* relative_pose_line_node_ = nullptr; + Ogre::SceneNode* error_line_node_ = nullptr; + Ogre::SceneNode* relative_pose_axes_node_ = nullptr; + Ogre::SceneNode* text_node_ = nullptr; std::shared_ptr relative_pose_line_; std::shared_ptr error_line_; std::shared_ptr relative_pose_axes_; - MovableText * text_; + MovableText* text_; CovarianceVisualPtr covariance_; std::string source_; - float loss_scale_{-1.0}; - float min_brightness_{0.0}; + float loss_scale_{ -1.0 }; + float min_brightness_{ 0.0 }; Ogre::ColourValue error_line_color_; bool visible_; @@ -216,14 +212,16 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - void setScale(const Ogre::Vector3 &) override {} - void setColor(float, float, float, float) override {} - const Ogre::Vector3 & getPosition() override; - const Ogre::Quaternion & getOrientation() override; - - Ogre::ColourValue computeLossErrorLineColor( - const Ogre::ColourValue & color, - const float loss_scale); + void setScale(const Ogre::Vector3&) override + { + } + void setColor(float, float, float, float) override + { + } + const Ogre::Vector3& getPosition() override; + const Ogre::Quaternion& getOrientation() override; + + Ogre::ColourValue computeLossErrorLineColor(const Ogre::ColourValue& color, const float loss_scale); // Make RelativePose2DStampedConstraintProperty friend class so it create // RelativePose2DStampedConstraintVisual objects diff --git a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp index 40086dbc7..bd8505a9b 100644 --- a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp +++ b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp @@ -65,8 +65,7 @@ class RelativePose2DStampedConstraintProperty; /** * @brief An rviz display for fuse_msgs::msg::SerializedGraph messages. */ -class SerializedGraphDisplay - : public rviz_common::MessageFilterDisplay +class SerializedGraphDisplay : public rviz_common::MessageFilterDisplay { Q_OBJECT @@ -84,7 +83,7 @@ class SerializedGraphDisplay void onDisable() override; - void load(const rviz_common::Config & config) override; + void load(const rviz_common::Config& config) override; private Q_SLOTS: void updateShowVariables(); @@ -93,18 +92,16 @@ private Q_SLOTS: private: using ChangedByUUIDMap = std::unordered_map; using ConstraintByUUIDMap = - std::unordered_map, - fuse_core::uuid::hash>; + std::unordered_map, fuse_core::uuid::hash>; using ColorBySourceMap = std::unordered_map; - using ConstraintPropertyBySourceMap = std::map; + using ConstraintPropertyBySourceMap = std::map; using ConfigBySourceMap = std::unordered_map; void clear(); void processMessage(fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) override; - Ogre::SceneNode * root_node_; + Ogre::SceneNode* root_node_; ConstraintByUUIDMap constraint_visuals_; @@ -113,9 +110,9 @@ private Q_SLOTS: ChangedByUUIDMap variables_changed_map_; ChangedByUUIDMap constraints_changed_map_; - BoolProperty * show_variables_property_; - BoolProperty * show_constraints_property_; - Pose2DStampedProperty * variable_property_; + BoolProperty* show_variables_property_; + BoolProperty* show_constraints_property_; + Pose2DStampedProperty* variable_property_; ConstraintPropertyBySourceMap constraint_source_properties_; ConfigBySourceMap constraint_source_configs_; diff --git a/fuse_viz/src/mapped_covariance_property.cpp b/fuse_viz/src/mapped_covariance_property.cpp index 4de425c0d..771e79a4a 100644 --- a/fuse_viz/src/mapped_covariance_property.cpp +++ b/fuse_viz/src/mapped_covariance_property.cpp @@ -52,94 +52,85 @@ using rviz_common::properties::ColorProperty; using rviz_common::properties::EnumProperty; using rviz_common::properties::FloatProperty; -MappedCovarianceProperty::MappedCovarianceProperty( - const QString & name, bool default_value, const QString & description, - Property * parent, const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) +MappedCovarianceProperty::MappedCovarianceProperty(const QString& name, bool default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { - position_property_ = new BoolProperty( - "Position", true, "Whether or not to show the position part of covariances", - this, SLOT(updateVisibility())); + position_property_ = new BoolProperty("Position", true, "Whether or not to show the position part of covariances", + this, SLOT(updateVisibility())); position_property_->setDisableChildrenIfFalse(true); position_color_property_ = - new ColorProperty( - "Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new ColorProperty("Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); - position_alpha_property_ = new FloatProperty( - "Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + position_alpha_property_ = new FloatProperty("Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); position_alpha_property_->setMin(0); position_alpha_property_->setMax(1); - position_scale_property_ = new FloatProperty( - "Scale", 1.0f, - "Scale factor to be applied to covariance ellipse. Corresponds to the " - "number of standard deviations to display", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + position_scale_property_ = new FloatProperty("Scale", 1.0f, + "Scale factor to be applied to covariance ellipse. Corresponds to the " + "number of standard deviations to display", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); position_scale_property_->setMin(0); orientation_property_ = - new BoolProperty( - "Orientation", true, "Whether or not to show the orientation part of covariances", this, - SLOT(updateVisibility())); + new BoolProperty("Orientation", true, "Whether or not to show the orientation part of covariances", this, + SLOT(updateVisibility())); orientation_property_->setDisableChildrenIfFalse(true); orientation_frame_property_ = - new EnumProperty( - "Frame", "Local", "The frame used to display the orientation covariance.", - orientation_property_, - SLOT(updateOrientationFrame()), this); + new EnumProperty("Frame", "Local", "The frame used to display the orientation covariance.", orientation_property_, + SLOT(updateOrientationFrame()), this); orientation_frame_property_->addOption("Local", Local); orientation_frame_property_->addOption("Fixed", Fixed); - orientation_colorstyle_property_ = new EnumProperty( - "Color Style", "Unique", - "Style to color the orientation covariance: XYZ with same unique " - "color or following RGB order", - orientation_property_, SLOT(updateColorStyleChoice()), this); + orientation_colorstyle_property_ = new EnumProperty("Color Style", "Unique", + "Style to color the orientation covariance: XYZ with same unique " + "color or following RGB order", + orientation_property_, SLOT(updateColorStyleChoice()), this); orientation_colorstyle_property_->addOption("Unique", Unique); orientation_colorstyle_property_->addOption("RGB", RGB); orientation_color_property_ = - new ColorProperty( - "Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.", orientation_property_, - SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new ColorProperty("Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.", orientation_property_, + SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_alpha_property_ = - new FloatProperty( - "Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", orientation_property_, - SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", orientation_property_, + SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_alpha_property_->setMin(0); orientation_alpha_property_->setMax(1); orientation_offset_property_ = - new FloatProperty( - "Offset", 1.0f, - "For 3D poses is the distance where to position the ellipses representing orientation " - "covariance. For 2D poses is the height of the triangle representing the variance on yaw", - orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Offset", 1.0f, + "For 3D poses is the distance where to position the ellipses representing orientation " + "covariance. For 2D poses is the height of the triangle representing the variance on yaw", + orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_offset_property_->setMin(0); orientation_scale_property_ = - new FloatProperty( - "Scale", 1.0f, - "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of " - "standard deviations to display", - orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Scale", 1.0f, + "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of " + "standard deviations to display", + orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_scale_property_->setMin(0); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -160,13 +151,13 @@ void MappedCovarianceProperty::updateColorStyleChoice() void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateColorAndAlphaAndScaleAndOffset(entry.second); } } -void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( - const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual) { float pos_alpha = position_alpha_property_->getFloat(); float pos_scale = position_scale_property_->getFloat(); @@ -177,10 +168,13 @@ void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( float ori_alpha = orientation_alpha_property_->getFloat(); float ori_offset = orientation_offset_property_->getFloat(); float ori_scale = orientation_scale_property_->getFloat(); - if (orientation_colorstyle_property_->getOptionInt() == Unique) { + if (orientation_colorstyle_property_->getOptionInt() == Unique) + { QColor ori_color = orientation_color_property_->getColor(); visual->setOrientationColor(ori_color.redF(), ori_color.greenF(), ori_color.blueF(), ori_alpha); - } else { + } + else + { visual->setOrientationColorToRGB(ori_alpha); } visual->setOrientationOffset(ori_offset); @@ -189,17 +183,21 @@ void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( void MappedCovarianceProperty::updateVisibility() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateVisibility(entry.second); } } -void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr& visual) { bool show_covariance = getBool(); - if (!show_covariance) { + if (!show_covariance) + { visual->setVisible(false); - } else { + } + else + { bool show_position_covariance = position_property_->getBool(); bool show_orientation_covariance = orientation_property_->getBool(); visual->setPositionVisible(show_position_covariance); @@ -209,18 +207,19 @@ void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr void MappedCovarianceProperty::updateOrientationFrame() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateOrientationFrame(entry.second); } } -void MappedCovarianceProperty::updateOrientationFrame(const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateOrientationFrame(const MappedCovarianceVisualPtr& visual) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); visual->setRotatingFrame(use_rotating_frame); } -void MappedCovarianceProperty::eraseVisual(const std::string & key) +void MappedCovarianceProperty::eraseVisual(const std::string& key) { covariances_.erase(key); } @@ -235,13 +234,12 @@ size_t MappedCovarianceProperty::sizeVisual() return covariances_.size(); } -MappedCovarianceProperty::MappedCovarianceVisualPtr MappedCovarianceProperty::createAndInsertVisual( - const std::string & key, Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) +MappedCovarianceProperty::MappedCovarianceVisualPtr +MappedCovarianceProperty::createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); - MappedCovarianceVisualPtr visual(new MappedCovarianceVisual( - scene_manager, parent_node, - use_rotating_frame)); + MappedCovarianceVisualPtr visual(new MappedCovarianceVisual(scene_manager, parent_node, use_rotating_frame)); updateVisibility(visual); updateOrientationFrame(visual); updateColorAndAlphaAndScaleAndOffset(visual); diff --git a/fuse_viz/src/mapped_covariance_visual.cpp b/fuse_viz/src/mapped_covariance_visual.cpp index d49205b0a..27e19f9d9 100644 --- a/fuse_viz/src/mapped_covariance_visual.cpp +++ b/fuse_viz/src/mapped_covariance_visual.cpp @@ -56,7 +56,7 @@ double deg2rad(double degrees) } // Local function to force the axis to be right handed for 3D. Taken from ecl_statistics -void makeRightHanded(Eigen::Matrix3d & eigenvectors, Eigen::Vector3d & eigenvalues) +void makeRightHanded(Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues) { // Note that sorting of eigenvalues may end up with left-hand coordinate system. // So here we correctly sort it so that it does end up being righ-handed and normalised. @@ -67,18 +67,21 @@ void makeRightHanded(Eigen::Matrix3d & eigenvectors, Eigen::Vector3d & eigenvalu Eigen::Vector3d c2 = eigenvectors.block<3, 1>(0, 2); c2.normalize(); Eigen::Vector3d cc = c0.cross(c1); - if (cc.dot(c2) < 0) { + if (cc.dot(c2) < 0) + { eigenvectors << c1, c0, c2; double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; - } else { + } + else + { eigenvectors << c0, c1, c2; } } // Local function to force the axis to be right handed for 2D. Based on the one from ecl_statistics -void makeRightHanded(Eigen::Matrix2d & eigenvectors, Eigen::Vector2d & eigenvalues) +void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues) { // Note that sorting of eigenvalues may end up with left-hand coordinate system. // So here we correctly sort it so that it does end up being righ-handed and normalised. @@ -91,19 +94,21 @@ void makeRightHanded(Eigen::Matrix2d & eigenvectors, Eigen::Vector2d & eigenvalu c1.head<2>() = eigenvectors.col(1); c1.normalize(); Eigen::Vector3d cc = c0.cross(c1); - if (cc[2] < 0) { + if (cc[2] < 0) + { eigenvectors << c1.head<2>(), c0.head<2>(); double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; - } else { + } + else + { eigenvectors << c0.head<2>(), c1.head<2>(); } } -void computeShapeScaleAndOrientation3D( - const Eigen::Matrix3d & covariance, Ogre::Vector3 & scale, - Ogre::Quaternion & orientation) +void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, + Ogre::Quaternion& orientation) { Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity()); Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero()); @@ -112,14 +117,16 @@ void computeShapeScaleAndOrientation3D( // matrix FIXME: Should we use Eigen's pseudoEigenvectors() ? Eigen::SelfAdjointEigenSolver eigensolver(covariance); // Compute eigenvectors and eigenvalues - if (eigensolver.info() == Eigen::Success) { + if (eigensolver.info() == Eigen::Success) + { eigenvalues = eigensolver.eigenvalues(); eigenvectors = eigensolver.eigenvectors(); - } else { + } + else + { static rclcpp::Clock clock; - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("fuse"), clock, 1000, - "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, + "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); eigenvalues = Eigen::Vector3d::Zero(); // Setting the scale to zero will hide it on the screen eigenvectors = Eigen::Matrix3d::Identity(); } @@ -128,11 +135,9 @@ void computeShapeScaleAndOrientation3D( makeRightHanded(eigenvectors, eigenvalues); // Define the rotation - orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), - eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2), - eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2))); + orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), + eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2), + eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2))); // Define the scale. eigenvalues are the variances, so we take the sqrt to draw the standard // deviation @@ -148,9 +153,8 @@ enum Plane XY_PLANE // normal is z-axis }; -void computeShapeScaleAndOrientation2D( - const Eigen::Matrix2d & covariance, Ogre::Vector3 & scale, - Ogre::Quaternion & orientation, Plane plane = XY_PLANE) +void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, + Ogre::Quaternion& orientation, Plane plane = XY_PLANE) { Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity()); Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero()); @@ -159,14 +163,16 @@ void computeShapeScaleAndOrientation2D( // matrix FIXME: Should we use Eigen's pseudoEigenvectors() ? Eigen::SelfAdjointEigenSolver eigensolver(covariance); // Compute eigenvectors and eigenvalues - if (eigensolver.info() == Eigen::Success) { + if (eigensolver.info() == Eigen::Success) + { eigenvalues = eigensolver.eigenvalues(); eigenvectors = eigensolver.eigenvectors(); - } else { + } + else + { static rclcpp::Clock clock; - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("fuse"), clock, 1000, - "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, + "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); eigenvalues = Eigen::Vector2d::Zero(); // Setting the scale to zero will hide it on the screen eigenvectors = Eigen::Matrix2d::Identity(); } @@ -177,29 +183,28 @@ void computeShapeScaleAndOrientation2D( // Define the rotation and scale of the plane // The Eigenvalues are the variances. The scales are two times the standard // deviation. The scale of the missing dimension is set to zero. - if (plane == YZ_PLANE) { + if (plane == YZ_PLANE) + { orientation.FromRotationMatrix( - Ogre::Matrix3( - 1, 0, 0, 0, eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), - eigenvectors(1, 1))); + Ogre::Matrix3(1, 0, 0, 0, eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1))); scale.x = 0; scale.y = 2 * std::sqrt(eigenvalues[0]); scale.z = 2 * std::sqrt(eigenvalues[1]); - } else if (plane == XZ_PLANE) { + } + else if (plane == XZ_PLANE) + { orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), 0, eigenvectors(0, 1), 0, 1, 0, eigenvectors(1, 0), 0, - eigenvectors(1, 1))); + Ogre::Matrix3(eigenvectors(0, 0), 0, eigenvectors(0, 1), 0, 1, 0, eigenvectors(1, 0), 0, eigenvectors(1, 1))); scale.x = 2 * std::sqrt(eigenvalues[0]); scale.y = 0; scale.z = 2 * std::sqrt(eigenvalues[1]); - } else { // plane == XY_PLANE + } + else + { // plane == XY_PLANE orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), - eigenvectors(1, 1), 0, 0, 0, 1)); + Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1), 0, 0, 0, 1)); scale.x = 2 * std::sqrt(eigenvalues[0]); scale.y = 2 * std::sqrt(eigenvalues[1]); @@ -207,10 +212,11 @@ void computeShapeScaleAndOrientation2D( } } -void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degrees) +void radianScaleToMetricScaleBounded(Ogre::Real& radian_scale, float max_degrees) { radian_scale /= 2.0; - if (radian_scale > deg2rad(max_degrees)) { + if (radian_scale > deg2rad(max_degrees)) + { radian_scale = deg2rad(max_degrees); } radian_scale = 2.0 * tan(radian_scale); @@ -220,12 +226,10 @@ void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degree const float MappedCovarianceVisual::max_degrees = 89.0; -MappedCovarianceVisual::MappedCovarianceVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - bool is_local_rotation, bool is_visible, float pos_scale, - float ori_scale, float ori_offset) -: Object(scene_manager), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_( - is_visible) +MappedCovarianceVisual::MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + bool is_local_rotation, bool is_visible, float pos_scale, + float ori_scale, float ori_offset) + : Object(scene_manager), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_(is_visible) { // Main node of the visual root_node_ = parent_node->createChildSceneNode(); @@ -236,34 +240,36 @@ MappedCovarianceVisual::MappedCovarianceVisual( position_scale_node_ = fixed_orientation_node_->createChildSceneNode(); // Node to be oriented and scaled from the message's covariance position_node_ = position_scale_node_->createChildSceneNode(); - position_shape_ = new rviz_rendering::Shape( - rviz_rendering::Shape::Sphere, scene_manager_, - position_node_); + position_shape_ = new rviz_rendering::Shape(rviz_rendering::Shape::Sphere, scene_manager_, position_node_); // Node to scale the orientation part of the covariance. May be attached to both the local (root) // node or the fixed frame node. May be re-attached later by setRotatingFrame() - if (local_rotation_) { + if (local_rotation_) + { orientation_root_node_ = root_node_->createChildSceneNode(); - } else { + } + else + { orientation_root_node_ = fixed_orientation_node_->createChildSceneNode(); } - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { // Node to position and orient the shape along the axis. One for each axis. orientation_offset_node_[i] = orientation_root_node_->createChildSceneNode(); // Does not inherit scale from the parent. This is needed to keep the cylinders with the same // height. The scale is set by setOrientationScale() orientation_offset_node_[i]->setInheritScale(false); - if (i != kYaw2D) { - orientation_shape_[i] = new rviz_rendering::Shape( - rviz_rendering::Shape::Cylinder, - scene_manager_, - orientation_offset_node_[i]); - } else { - orientation_shape_[i] = new rviz_rendering::Shape( - rviz_rendering::Shape::Cone, scene_manager_, - orientation_offset_node_[i]); + if (i != kYaw2D) + { + orientation_shape_[i] = + new rviz_rendering::Shape(rviz_rendering::Shape::Cylinder, scene_manager_, orientation_offset_node_[i]); + } + else + { + orientation_shape_[i] = + new rviz_rendering::Shape(rviz_rendering::Shape::Cone, scene_manager_, orientation_offset_node_[i]); } // Initialize all current scales to 0 @@ -273,21 +279,14 @@ MappedCovarianceVisual::MappedCovarianceVisual( // Position the cylindes at position 1.0 in the respective axis, and perpendicular to the axis. // x-axis (roll) orientation_offset_node_[kRoll]->setPosition(Ogre::Vector3::UNIT_X); - orientation_offset_node_[kRoll]->setOrientation( - Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X) * - Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); + orientation_offset_node_[kRoll]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X) * + Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); // y-axis (pitch) orientation_offset_node_[kPitch]->setPosition(Ogre::Vector3(Ogre::Vector3::UNIT_Y)); - orientation_offset_node_[kPitch]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_Y)); + orientation_offset_node_[kPitch]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Y)); // z-axis (yaw) orientation_offset_node_[kYaw]->setPosition(Ogre::Vector3(Ogre::Vector3::UNIT_Z)); - orientation_offset_node_[kYaw]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_X)); + orientation_offset_node_[kYaw]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X)); // z-axis (yaw 2D) // NOTE: rviz use a cone defined by the file rviz/ogre_media/models/rviz_cone.mesh, and it's // origin is not at the top of the cone. Since we want the top to be at the origin of @@ -299,10 +298,7 @@ MappedCovarianceVisual::MappedCovarianceVisual( // something like a 2D "pie slice" and use it instead of the cone. static const double cone_origin_to_top = 0.49115; orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X); - orientation_offset_node_[kYaw2D]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_Z)); + orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); // set initial visibility and scale // root node is always visible. The visibility will be updated on its childs. @@ -317,7 +313,8 @@ MappedCovarianceVisual::~MappedCovarianceVisual() delete position_shape_; scene_manager_->destroySceneNode(position_node_); - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { delete orientation_shape_[i]; scene_manager_->destroySceneNode(orientation_offset_node_[i]); } @@ -327,20 +324,25 @@ MappedCovarianceVisual::~MappedCovarianceVisual() scene_manager_->destroySceneNode(root_node_); } -void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose) +void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose) { // check for NaN in covariance - for (unsigned i = 0; i < 3; ++i) { - if (std::isnan(pose.covariance[i])) { + for (unsigned i = 0; i < 3; ++i) + { + if (std::isnan(pose.covariance[i])) + { static rclcpp::Clock clock; RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, "covariance contains NaN"); return; } } - if (pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0) { + if (pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0) + { pose_2d_ = true; - } else { + } + else + { pose_2d_ = false; } @@ -357,48 +359,52 @@ void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCov Eigen::Map> covariance(pose.covariance.data()); updatePosition(covariance); - if (!pose_2d_) { + if (!pose_2d_) + { updateOrientation(covariance, kRoll); updateOrientation(covariance, kPitch); updateOrientation(covariance, kYaw); - } else { + } + else + { updateOrientation(covariance, kYaw2D); } } -void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d & covariance) +void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d& covariance) { // Compute shape and orientation for the position part of covariance Ogre::Vector3 shape_scale; Ogre::Quaternion shape_orientation; - if (pose_2d_) { - computeShapeScaleAndOrientation2D( - covariance.topLeftCorner<2, - 2>(), shape_scale, shape_orientation, XY_PLANE); + if (pose_2d_) + { + computeShapeScaleAndOrientation2D(covariance.topLeftCorner<2, 2>(), shape_scale, shape_orientation, XY_PLANE); // Make the scale in z minimal for better visualization shape_scale.z = 0.001; - } else { - computeShapeScaleAndOrientation3D( - covariance.topLeftCorner<3, - 3>(), shape_scale, shape_orientation); + } + else + { + computeShapeScaleAndOrientation3D(covariance.topLeftCorner<3, 3>(), shape_scale, shape_orientation); } // Rotate and scale the position scene node position_node_->setOrientation(shape_orientation); - if (!shape_scale.isNaN()) { + if (!shape_scale.isNaN()) + { position_node_->setScale(shape_scale); - } else { - RCLCPP_WARN_STREAM( - rclcpp::get_logger( - "fuse"), "position shape_scale contains NaN: " << shape_scale); + } + else + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("fuse"), "position shape_scale contains NaN: " << shape_scale); } } -void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index) +void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index) { Ogre::Vector3 shape_scale; Ogre::Quaternion shape_orientation; // Compute shape and orientation for the orientation shape - if (pose_2d_) { + if (pose_2d_) + { // We should only enter on this scope if the index is kYaw2D assert(index == kYaw2D); // 2D poses only depend on yaw. @@ -415,16 +421,23 @@ void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covarianc // So we need to convert it to the linear scale of the shape using tan(). // Also, we bound the maximum std radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); - } else { + } + else + { assert(index != kYaw2D); // Get the correct sub-matrix based on the index Eigen::Matrix2d covarianceAxis; - if (index == kRoll) { + if (index == kRoll) + { covarianceAxis = covariance.bottomRightCorner<2, 2>(); - } else if (index == kPitch) { + } + else if (index == kPitch) + { covarianceAxis << covariance(3, 3), covariance(3, 5), covariance(5, 3), covariance(5, 5); - } else if (index == kYaw) { + } + else if (index == kYaw) + { covarianceAxis = covariance.block<2, 2>(3, 3); } @@ -447,12 +460,13 @@ void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covarianc // Rotate and scale the scene node of the orientation part orientation_shape_[index]->setOrientation(shape_orientation); - if (!shape_scale.isNaN()) { + if (!shape_scale.isNaN()) + { orientation_shape_[index]->setScale(shape_scale); - } else { - RCLCPP_WARN_STREAM( - rclcpp::get_logger( - "fuse"), "orientation shape_scale contains NaN: " << shape_scale); + } + else + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("fuse"), "orientation shape_scale contains NaN: " << shape_scale); } } @@ -464,9 +478,12 @@ void MappedCovarianceVisual::setScales(float pos_scale, float ori_scale) void MappedCovarianceVisual::setPositionScale(float pos_scale) { - if (pose_2d_) { + if (pose_2d_) + { position_scale_node_->setScale(pos_scale, pos_scale, 1.0); - } else { + } + else + { position_scale_node_->setScale(pos_scale, pos_scale, pos_scale); } } @@ -479,12 +496,16 @@ void MappedCovarianceVisual::setOrientationOffset(float ori_offset) // standard deviation when displayed with an scale of 1.0 // NOTE: We only want to change the scales of the dimensions that represent the // orientation covariance. The other dimensions are set to 1.0. - for (int i = 0; i < kNumOriShapes; i++) { - if (i == kYaw2D) { + for (int i = 0; i < kNumOriShapes; i++) + { + if (i == kYaw2D) + { // For 2D, the angle is only encoded on x, but we also scale on y to put the top of the cone // at the pose origin orientation_offset_node_[i]->setScale(ori_offset, ori_offset, 1.0); - } else { + } + else + { // For 3D, the angle covariance is encoded on x and z dimensions orientation_offset_node_[i]->setScale(ori_offset, 1.0, ori_offset); } @@ -497,16 +518,20 @@ void MappedCovarianceVisual::setOrientationScale(float ori_scale) // convert it to meters and apply to the shape scale. Note we have different invariant // scales in the 3D and in 2D. current_ori_scale_factor_ = ori_scale; - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { // Recover the last computed scale Ogre::Vector3 shape_scale = current_ori_scale_[i]; - if (i == kYaw2D) { + if (i == kYaw2D) + { // Changes in scale in 2D only affects the x dimension // Apply the current scale factor shape_scale.x *= current_ori_scale_factor_; // Convert from radians to meters radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); - } else { + } + else + { // Changes in scale in 3D only affects the x and z dimensions // Apply the current scale factor shape_scale.x *= current_ori_scale_factor_; @@ -520,14 +545,15 @@ void MappedCovarianceVisual::setOrientationScale(float ori_scale) } } -void MappedCovarianceVisual::setPositionColor(const Ogre::ColourValue & c) +void MappedCovarianceVisual::setPositionColor(const Ogre::ColourValue& c) { position_shape_->setColor(c); } -void MappedCovarianceVisual::setOrientationColor(const Ogre::ColourValue & c) +void MappedCovarianceVisual::setOrientationColor(const Ogre::ColourValue& c) { - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { orientation_shape_[i]->setColor(c); } } @@ -550,20 +576,21 @@ void MappedCovarianceVisual::setOrientationColor(float r, float g, float b, floa setOrientationColor(Ogre::ColourValue(r, g, b, a)); } -const Ogre::Vector3 & MappedCovarianceVisual::getPositionCovarianceScale() +const Ogre::Vector3& MappedCovarianceVisual::getPositionCovarianceScale() { return position_node_->getScale(); } -const Ogre::Quaternion & MappedCovarianceVisual::getPositionCovarianceOrientation() +const Ogre::Quaternion& MappedCovarianceVisual::getPositionCovarianceOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setUserData(const Ogre::Any & data) +void MappedCovarianceVisual::setUserData(const Ogre::Any& data) { position_shape_->setUserData(data); - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { orientation_shape_[i]->setUserData(data); } } @@ -593,42 +620,46 @@ void MappedCovarianceVisual::updateOrientationVisibility() orientation_offset_node_[kYaw2D]->setVisible(orientation_visible_ && pose_2d_); } -const Ogre::Vector3 & MappedCovarianceVisual::getPosition() +const Ogre::Vector3& MappedCovarianceVisual::getPosition() { return position_node_->getPosition(); } -const Ogre::Quaternion & MappedCovarianceVisual::getOrientation() +const Ogre::Quaternion& MappedCovarianceVisual::getOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setPosition(const Ogre::Vector3 & position) +void MappedCovarianceVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void MappedCovarianceVisual::setOrientation(const Ogre::Quaternion & orientation) +void MappedCovarianceVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } void MappedCovarianceVisual::setRotatingFrame(bool is_local_rotation) { - if (local_rotation_ == is_local_rotation) { + if (local_rotation_ == is_local_rotation) + { return; } local_rotation_ = is_local_rotation; - if (local_rotation_) { + if (local_rotation_) + { root_node_->addChild(fixed_orientation_node_->removeChild(orientation_root_node_->getName())); - } else { + } + else + { fixed_orientation_node_->addChild(root_node_->removeChild(orientation_root_node_->getName())); } } -rviz_rendering::Shape * MappedCovarianceVisual::getOrientationShape(ShapeIndex index) +rviz_rendering::Shape* MappedCovarianceVisual::getOrientationShape(ShapeIndex index) { return orientation_shape_[index]; } diff --git a/fuse_viz/src/pose_2d_stamped_property.cpp b/fuse_viz/src/pose_2d_stamped_property.cpp index 226751336..378c764ad 100644 --- a/fuse_viz/src/pose_2d_stamped_property.cpp +++ b/fuse_viz/src/pose_2d_stamped_property.cpp @@ -51,55 +51,45 @@ using rviz_common::properties::ColorProperty; using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; -Pose2DStampedProperty::Pose2DStampedProperty( - const QString & name, bool default_value, const QString & description, - Property * parent, const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) +Pose2DStampedProperty::Pose2DStampedProperty(const QString& name, bool default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { - color_property_ = new ColorProperty( - "Color", QColor(255, 0, 0), "Color to draw the variable sphere.", this, - SLOT(updateSphereColorAlpha())); + color_property_ = new ColorProperty("Color", QColor(255, 0, 0), "Color to draw the variable sphere.", this, + SLOT(updateSphereColorAlpha())); sphere_alpha_property_ = - new FloatProperty( - "Sphere Alpha", 1.0, "Alpha of variable sphere.", this, - SLOT(updateSphereColorAlpha())); + new FloatProperty("Sphere Alpha", 1.0, "Alpha of variable sphere.", this, SLOT(updateSphereColorAlpha())); sphere_alpha_property_->setMin(0.0); sphere_alpha_property_->setMax(1.0); - axes_alpha_property_ = - new FloatProperty( - "Axes Alpha", 0.0, "Alpha of variable axes.", this, - SLOT(updateAxesAlpha())); + axes_alpha_property_ = new FloatProperty("Axes Alpha", 0.0, "Alpha of variable axes.", this, SLOT(updateAxesAlpha())); axes_alpha_property_->setMin(0.0); axes_alpha_property_->setMax(1.0); - scale_property_ = new FloatProperty( - "Scale", 1.0, "Scale of variable sphere and axes.", this, SLOT( - updateScale())); + scale_property_ = new FloatProperty("Scale", 1.0, "Scale of variable sphere and axes.", this, SLOT(updateScale())); scale_property_->setMin(0.0); show_text_property_ = - new BoolProperty( - "Show Text", false, "Show variable type and UUID.", this, SLOT( - updateShowText())); - - text_scale_property_ = - new FloatProperty( - "Text Scale", 1.0, "Scale of variable text.", this, - SLOT(updateTextScale())); + new BoolProperty("Show Text", false, "Show variable type and UUID.", this, SLOT(updateShowText())); + + text_scale_property_ = new FloatProperty("Text Scale", 1.0, "Scale of variable text.", this, SLOT(updateTextScale())); text_scale_property_->setMin(0.0); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -107,16 +97,19 @@ Pose2DStampedProperty::Pose2DStampedProperty( setDisableChildrenIfFalse(true); } -Pose2DStampedProperty::VisualPtr Pose2DStampedProperty::createAndInsertOrUpdateVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +Pose2DStampedProperty::VisualPtr +Pose2DStampedProperty::createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { - auto & visual = variables_[position.uuid()]; + auto& visual = variables_[position.uuid()]; - if (visual) { + if (visual) + { visual->setPose2DStamped(position, orientation); - } else { + } + else + { visual.reset(new Visual(scene_manager, parent_node, position, orientation)); visual->setPose2DStamped(position, orientation); @@ -132,7 +125,7 @@ Pose2DStampedProperty::VisualPtr Pose2DStampedProperty::createAndInsertOrUpdateV return visual; } -void Pose2DStampedProperty::eraseVisual(const fuse_core::UUID & uuid) +void Pose2DStampedProperty::eraseVisual(const fuse_core::UUID& uuid) { variables_.erase(uuid); } @@ -144,76 +137,80 @@ void Pose2DStampedProperty::clearVisual() void Pose2DStampedProperty::updateVisibility() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateVisibility(entry.second); } } void Pose2DStampedProperty::updateAxesAlpha() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateAxesAlpha(entry.second); } } void Pose2DStampedProperty::updateScale() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateScale(entry.second); } } void Pose2DStampedProperty::updateShowText() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateShowText(entry.second); } } void Pose2DStampedProperty::updateSphereColorAlpha() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateSphereColorAlpha(entry.second); } } void Pose2DStampedProperty::updateTextScale() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateTextScale(entry.second); } } -void Pose2DStampedProperty::updateAxesAlpha(const VisualPtr & variable) +void Pose2DStampedProperty::updateAxesAlpha(const VisualPtr& variable) { variable->setAxesAlpha(axes_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateScale(const VisualPtr & variable) +void Pose2DStampedProperty::updateScale(const VisualPtr& variable) { - variable->setScale(Ogre::Vector3{scale_property_->getFloat()}); // NOLINT(whitespace/braces) + variable->setScale(Ogre::Vector3{ scale_property_->getFloat() }); // NOLINT(whitespace/braces) } -void Pose2DStampedProperty::updateShowText(const VisualPtr & variable) +void Pose2DStampedProperty::updateShowText(const VisualPtr& variable) { variable->setTextVisible(show_text_property_->getBool()); } -void Pose2DStampedProperty::updateSphereColorAlpha(const VisualPtr & variable) +void Pose2DStampedProperty::updateSphereColorAlpha(const VisualPtr& variable) { const auto color = color_property_->getColor(); - variable->setSphereColor( - color.redF(), color.greenF(), - color.blueF(), sphere_alpha_property_->getFloat()); + variable->setSphereColor(color.redF(), color.greenF(), color.blueF(), sphere_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateTextScale(const VisualPtr & variable) +void Pose2DStampedProperty::updateTextScale(const VisualPtr& variable) { - variable->setTextScale(Ogre::Vector3{text_scale_property_->getFloat()}); // NOLINT + variable->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void Pose2DStampedProperty::updateVisibility(const VisualPtr & variable) +void Pose2DStampedProperty::updateVisibility(const VisualPtr& variable) { const auto visible = getBool(); diff --git a/fuse_viz/src/pose_2d_stamped_visual.cpp b/fuse_viz/src/pose_2d_stamped_visual.cpp index 19f4a1773..f9e2c4f3b 100644 --- a/fuse_viz/src/pose_2d_stamped_visual.cpp +++ b/fuse_viz/src/pose_2d_stamped_visual.cpp @@ -50,17 +50,14 @@ namespace fuse_viz { using rviz_rendering::MovableText; -Pose2DStampedVisual::Pose2DStampedVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, const bool visible) -: Object(scene_manager), root_node_(parent_node->createChildSceneNode()), visible_(visible) +Pose2DStampedVisual::Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const bool visible) + : Object(scene_manager), root_node_(parent_node->createChildSceneNode()), visible_(visible) { // Create sphere: sphere_node_ = root_node_->createChildSceneNode(); - sphere_ = std::make_shared( - rviz_rendering::Shape::Sphere, scene_manager_, - sphere_node_); + sphere_ = std::make_shared(rviz_rendering::Shape::Sphere, scene_manager_, sphere_node_); setSphereColor(1.0, 0.0, 0.0, 1.0); // Create axes: @@ -69,7 +66,7 @@ Pose2DStampedVisual::Pose2DStampedVisual( // Create text: const auto caption = position.type() + "::" + fuse_core::uuid::to_string(position.uuid()) + '\n' + - orientation.type() + "::" + fuse_core::uuid::to_string(orientation.uuid()); + orientation.type() + "::" + fuse_core::uuid::to_string(orientation.uuid()); text_ = new MovableText(caption); text_->setCaption(caption); text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE); @@ -96,14 +93,13 @@ Pose2DStampedVisual::~Pose2DStampedVisual() scene_manager_->destroySceneNode(root_node_); } -void Pose2DStampedVisual::setPose2DStamped( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +void Pose2DStampedVisual::setPose2DStamped(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { setPose2DStamped(toOgre(position), toOgre(orientation)); } -void Pose2DStampedVisual::setUserData(const Ogre::Any & data) +void Pose2DStampedVisual::setUserData(const Ogre::Any& data) { axes_->setUserData(data); sphere_->setUserData(data); @@ -116,31 +112,25 @@ void Pose2DStampedVisual::setSphereColor(const float r, const float g, const flo void Pose2DStampedVisual::setAxesAlpha(const float alpha) { - static const auto & default_x_color_ = axes_->getDefaultXColor(); - static const auto & default_y_color_ = axes_->getDefaultYColor(); - static const auto & default_z_color_ = axes_->getDefaultZColor(); - - axes_->setXColor( - Ogre::ColourValue( - default_x_color_.r, default_x_color_.g, default_x_color_.b, - alpha)); // NOLINT - axes_->setYColor( - Ogre::ColourValue( - default_y_color_.r, default_y_color_.g, default_y_color_.b, - alpha)); // NOLINT - axes_->setZColor( - Ogre::ColourValue( - default_z_color_.r, default_z_color_.g, default_z_color_.b, - alpha)); // NOLINT + static const auto& default_x_color_ = axes_->getDefaultXColor(); + static const auto& default_y_color_ = axes_->getDefaultYColor(); + static const auto& default_z_color_ = axes_->getDefaultZColor(); + + axes_->setXColor(Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, + alpha)); // NOLINT + axes_->setYColor(Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, + alpha)); // NOLINT + axes_->setZColor(Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, + alpha)); // NOLINT } -void Pose2DStampedVisual::setScale(const Ogre::Vector3 & scale) +void Pose2DStampedVisual::setScale(const Ogre::Vector3& scale) { sphere_->setScale(scale); axes_->setScale(scale); } -void Pose2DStampedVisual::setTextScale(const Ogre::Vector3 & scale) +void Pose2DStampedVisual::setTextScale(const Ogre::Vector3& scale) { text_node_->setScale(scale); } @@ -156,9 +146,7 @@ void Pose2DStampedVisual::setVisible(const bool visible) axes_node_->setVisible(visible); } -void Pose2DStampedVisual::setPose2DStamped( - const Ogre::Vector3 & position, - const Ogre::Quaternion & orientation) +void Pose2DStampedVisual::setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) { axes_->setPosition(position); axes_->setOrientation(orientation); @@ -166,22 +154,22 @@ void Pose2DStampedVisual::setPose2DStamped( text_node_->setPosition(position); } -const Ogre::Vector3 & Pose2DStampedVisual::getPosition() +const Ogre::Vector3& Pose2DStampedVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion & Pose2DStampedVisual::getOrientation() +const Ogre::Quaternion& Pose2DStampedVisual::getOrientation() { return root_node_->getOrientation(); } -void Pose2DStampedVisual::setPosition(const Ogre::Vector3 & position) +void Pose2DStampedVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void Pose2DStampedVisual::setOrientation(const Ogre::Quaternion & orientation) +void Pose2DStampedVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp index 7b3381d98..1816b5af2 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp @@ -54,81 +54,71 @@ using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty( - const QString & name, bool default_value, const QString & description, Property * parent, - const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) + const QString& name, bool default_value, const QString& description, Property* parent, const char* changed_slot, + QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { color_property_ = - new ColorProperty( - "Color", QColor(255, 255, 255), "Color to draw the constraint relative pose and error lines.", - this, SLOT(updateColor())); + new ColorProperty("Color", QColor(255, 255, 255), "Color to draw the constraint relative pose and error lines.", + this, SLOT(updateColor())); - relative_pose_axes_alpha_property_ = new FloatProperty( - "Axes Alpha", 0.0, "Alpha of constraint relative pose axes.", - this, SLOT(updateRelativePoseAxesAlpha())); + relative_pose_axes_alpha_property_ = new FloatProperty("Axes Alpha", 0.0, "Alpha of constraint relative pose axes.", + this, SLOT(updateRelativePoseAxesAlpha())); relative_pose_axes_alpha_property_->setMin(0.0); relative_pose_axes_alpha_property_->setMax(1.0); - relative_pose_axes_scale_property_ = new FloatProperty( - "Axes Scale", 1.0, "Scale of constraint relative pose axes.", - this, SLOT(updateRelativePoseAxesScale())); + relative_pose_axes_scale_property_ = new FloatProperty("Axes Scale", 1.0, "Scale of constraint relative pose axes.", + this, SLOT(updateRelativePoseAxesScale())); relative_pose_axes_scale_property_->setMin(0.0); - relative_pose_line_alpha_property_ = new FloatProperty( - "Line Alpha", 1.0, "Alpha of constraint relative pose line.", - this, SLOT(updateRelativePoseLineAlpha())); + relative_pose_line_alpha_property_ = new FloatProperty("Line Alpha", 1.0, "Alpha of constraint relative pose line.", + this, SLOT(updateRelativePoseLineAlpha())); relative_pose_line_alpha_property_->setMin(0.0); relative_pose_line_alpha_property_->setMax(1.0); relative_pose_line_width_property_ = new FloatProperty( - "Line Width", 0.1, "Line width of constraint relative pose line.", this, - SLOT(updateRelativePoseLineWidth())); + "Line Width", 0.1, "Line width of constraint relative pose line.", this, SLOT(updateRelativePoseLineWidth())); relative_pose_line_width_property_->setMin(0.0); error_line_alpha_property_ = - new FloatProperty( - "Error Line Alpha", 0.5, "Alpha of constraint error line.", this, - SLOT(updateErrorLineAlpha())); + new FloatProperty("Error Line Alpha", 0.5, "Alpha of constraint error line.", this, SLOT(updateErrorLineAlpha())); error_line_alpha_property_->setMin(0.0); error_line_alpha_property_->setMax(1.0); - error_line_width_property_ = new FloatProperty( - "Error Line Width", 0.1, "Line width of constraint error line.", this, - SLOT(updateErrorLineWidth())); + error_line_width_property_ = new FloatProperty("Error Line Width", 0.1, "Line width of constraint error line.", this, + SLOT(updateErrorLineWidth())); error_line_width_property_->setMin(0.0); - loss_min_brightness_property_ = new FloatProperty( - "Loss Min Brightness", 0.25, - "Min brightness to show the loss impact on the constraint error " - "line.", - this, SLOT(updateLossMinBrightness())); + loss_min_brightness_property_ = new FloatProperty("Loss Min Brightness", 0.25, + "Min brightness to show the loss impact on the constraint error " + "line.", + this, SLOT(updateLossMinBrightness())); loss_min_brightness_property_->setMin(0.0); loss_min_brightness_property_->setMax(1.0); show_text_property_ = - new BoolProperty( - "Show Text", false, "Show constraint source, type and UUID.", this, - SLOT(updateShowText())); - - text_scale_property_ = - new FloatProperty( - "Text Scale", 1.0, "Scale of variable text.", this, - SLOT(updateTextScale())); + new BoolProperty("Show Text", false, "Show constraint source, type and UUID.", this, SLOT(updateShowText())); + + text_scale_property_ = new FloatProperty("Text Scale", 1.0, "Scale of variable text.", this, SLOT(updateTextScale())); text_scale_property_->setMin(0.0); covariance_property_ = new MappedCovarianceProperty( - "Covariance", true, "Whether or not the constraint covariance should be shown.", this); + "Covariance", true, "Whether or not the constraint covariance should be shown.", this); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -136,19 +126,15 @@ RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty setDisableChildrenIfFalse(true); } -RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstraintProperty:: -createAndInsertVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph) +RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstraintProperty::createAndInsertVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) { - VisualPtr visual{new Visual(scene_manager, parent_node, constraint)}; + VisualPtr visual{ new Visual(scene_manager, parent_node, constraint) }; constraints_[constraint.uuid()] = visual; - visual->setCovariance( - covariance_property_->createAndInsertVisual( - fuse_core::uuid::to_string(constraint.uuid()), - scene_manager, parent_node)); + visual->setCovariance(covariance_property_->createAndInsertVisual(fuse_core::uuid::to_string(constraint.uuid()), + scene_manager, parent_node)); visual->setConstraint(constraint, graph); updateColor(visual); @@ -166,7 +152,7 @@ createAndInsertVisual( return visual; } -void RelativePose2DStampedConstraintProperty::eraseVisual(const fuse_core::UUID & uuid) +void RelativePose2DStampedConstraintProperty::eraseVisual(const fuse_core::UUID& uuid) { covariance_property_->eraseVisual(fuse_core::uuid::to_string(uuid)); constraints_.erase(uuid); @@ -178,161 +164,161 @@ void RelativePose2DStampedConstraintProperty::clearVisual() constraints_.clear(); } -void RelativePose2DStampedConstraintProperty::setColor(const QColor & color) +void RelativePose2DStampedConstraintProperty::setColor(const QColor& color) { color_property_->setColor(color); } void RelativePose2DStampedConstraintProperty::updateVisibility() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateVisibility(entry.second); } } void RelativePose2DStampedConstraintProperty::updateColor() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateColor(entry.second); } } void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateErrorLineAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateErrorLineWidth() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateErrorLineWidth(entry.second); } } void RelativePose2DStampedConstraintProperty::updateLossMinBrightness() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateLossMinBrightness(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseAxesAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseAxesScale(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseLineAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseLineWidth(entry.second); } } void RelativePose2DStampedConstraintProperty::updateShowText() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateShowText(entry.second); } } void RelativePose2DStampedConstraintProperty::updateTextScale() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateTextScale(entry.second); } } -void RelativePose2DStampedConstraintProperty::updateColor(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateColor(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setRelativePoseLineColor( - color.redF(), color.greenF(), color.blueF(), - relative_pose_line_alpha_property_->getFloat()); - constraint->setErrorLineColor( - color.redF(), color.greenF(), - color.blueF(), error_line_alpha_property_->getFloat()); + constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), + relative_pose_line_alpha_property_->getFloat()); + constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setErrorLineColor( - color.redF(), color.greenF(), - color.blueF(), error_line_alpha_property_->getFloat()); + constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualPtr& constraint) { constraint->setErrorLineWidth(error_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr& constraint) { constraint->setLossMinBrightness(loss_min_brightness_property_->getFloat()); updateErrorLineAlpha(constraint); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha(const VisualPtr& constraint) { constraint->setRelativePoseAxesAlpha(relative_pose_axes_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale(const VisualPtr& constraint) { - constraint->setRelativePoseAxesScale( - Ogre::Vector3{relative_pose_axes_scale_property_->getFloat()}); + constraint->setRelativePoseAxesScale(Ogre::Vector3{ relative_pose_axes_scale_property_->getFloat() }); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setRelativePoseLineColor( - color.redF(), color.greenF(), color.blueF(), - relative_pose_line_alpha_property_->getFloat()); + constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), + relative_pose_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth(const VisualPtr& constraint) { constraint->setRelativePoseLineWidth(relative_pose_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateShowText(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateShowText(const VisualPtr& constraint) { constraint->setTextVisible(show_text_property_->getBool()); } -void RelativePose2DStampedConstraintProperty::updateTextScale(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateTextScale(const VisualPtr& constraint) { - constraint->setTextScale(Ogre::Vector3{text_scale_property_->getFloat()}); // NOLINT + constraint->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void RelativePose2DStampedConstraintProperty::updateVisibility(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateVisibility(const VisualPtr& constraint) { const auto visible = getBool(); diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp index 611c7297f..9beb1b6cf 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp @@ -66,16 +66,15 @@ namespace fuse_viz * @param[in] constraint A constraint * @return The constraint name of the form: source@type::uuid */ -std::string constraint_name(const fuse_constraints::RelativePose2DStampedConstraint & constraint) +std::string constraint_name(const fuse_constraints::RelativePose2DStampedConstraint& constraint) { - return constraint.source() + '@' + constraint.type() + "::" + fuse_core::uuid::to_string( - constraint.uuid()); + return constraint.source() + '@' + constraint.type() + "::" + fuse_core::uuid::to_string(constraint.uuid()); } RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, const bool visible) -: Object(scene_manager) + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const bool visible) + : Object(scene_manager) , root_node_(parent_node->createChildSceneNode()) , source_(constraint.source()) , visible_(visible) @@ -94,9 +93,7 @@ RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( // Create constraint relative pose axes: relative_pose_axes_node_ = root_node_->createChildSceneNode(); - relative_pose_axes_ = std::make_shared( - scene_manager_, - relative_pose_axes_node_, 10.0, 1.0); + relative_pose_axes_ = std::make_shared(scene_manager_, relative_pose_axes_node_, 10.0, 1.0); // Draw text: const auto caption = constraint_name(constraint); @@ -125,17 +122,16 @@ RelativePose2DStampedConstraintVisual::~RelativePose2DStampedConstraintVisual() } void RelativePose2DStampedConstraintVisual::setConstraint( - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph) + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) { // Update constraint relative pose line: - const auto & variables = constraint.variables(); + const auto& variables = constraint.variables(); const auto pose1 = getPose(graph, variables.at(0), variables.at(1)); - const auto & delta = constraint.delta(); - const tf2::Transform pose_delta{tf2::Quaternion{tf2::Vector3{0, 0, 1}, delta[2]}, - tf2::Vector3{delta[0], delta[1], 0}}; + const auto& delta = constraint.delta(); + const tf2::Transform pose_delta{ tf2::Quaternion{ tf2::Vector3{ 0, 0, 1 }, delta[2] }, + tf2::Vector3{ delta[0], delta[1], 0 } }; const auto absolute_pose = pose1 * pose_delta; const auto absolute_position_ogre = toOgre(absolute_pose.getOrigin()); @@ -162,14 +158,15 @@ void RelativePose2DStampedConstraintVisual::setConstraint( // Set error line color brightness based on the loss function impact on the constraint cost: auto loss_function = constraint.lossFunction(); - if (loss_function) { + if (loss_function) + { // Evaluate cost function without loss: - const double position1[] = {pose1.getOrigin().getX(), pose1.getOrigin().getY()}; - const double yaw1[] = {tf2::getYaw(pose1.getRotation())}; - const double position2[] = {pose2.getOrigin().getX(), pose2.getOrigin().getY()}; - const double yaw2[] = {tf2::getYaw(pose2.getRotation())}; + const double position1[] = { pose1.getOrigin().getX(), pose1.getOrigin().getY() }; + const double yaw1[] = { tf2::getYaw(pose1.getRotation()) }; + const double position2[] = { pose2.getOrigin().getX(), pose2.getOrigin().getY() }; + const double yaw2[] = { tf2::getYaw(pose2.getRotation()) }; - const double * parameters[] = {position1, yaw1, position2, yaw2}; + const double* parameters[] = { position1, yaw1, position2, yaw2 }; auto cost_function = constraint.costFunction(); @@ -200,16 +197,14 @@ void RelativePose2DStampedConstraintVisual::setConstraint( loss_function->Evaluate(squared_norm, rho); delete loss_function; - if (rho[0] > squared_norm) { + if (rho[0] > squared_norm) + { static rclcpp::Clock clock; - RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger( - "fuse"), clock, 10.0 * 1000, - "Detected invalid loss value of " << rho[0] << " greater than squared residual of " - << squared_norm << " for constraint " << - constraint_name(constraint) - << " with loss type " << constraint.loss()->type() - << ". Loss value clamped to the squared residual."); + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("fuse"), clock, 10.0 * 1000, + "Detected invalid loss value of " + << rho[0] << " greater than squared residual of " << squared_norm + << " for constraint " << constraint_name(constraint) << " with loss type " + << constraint.loss()->type() << ". Loss value clamped to the squared residual."); rho[0] = squared_norm; } @@ -227,9 +222,8 @@ void RelativePose2DStampedConstraintVisual::setConstraint( // Compute error line color with the loss function impact: const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); - error_line_->setColor( - loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, - error_line_color_.a); + error_line_->setColor(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, + error_line_color_.a); } // Update constraint relative pose axes: @@ -240,7 +234,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( text_node_->setPosition(absolute_position_ogre); } -void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any & data) +void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any& data) { relative_pose_line_->setUserData(data); error_line_->setUserData(data); @@ -263,16 +257,13 @@ void RelativePose2DStampedConstraintVisual::setLossMinBrightness(const float min min_brightness_ = min_brightness; } -void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor( - const float r, const float g, const float b, - const float a) +void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(const float r, const float g, const float b, + const float a) { relative_pose_line_->setColor(r, g, b, a); } -void RelativePose2DStampedConstraintVisual::setErrorLineColor( - const float r, const float g, const float b, - const float a) +void RelativePose2DStampedConstraintVisual::setErrorLineColor(const float r, const float g, const float b, const float a) { // Cache error line color w/o the loss function impact, so we can change its darkness based on the // loss function impact on the constraint cost: Note that we cannot recover/retrieve the color @@ -284,31 +275,30 @@ void RelativePose2DStampedConstraintVisual::setErrorLineColor( // Compute error line color with the impact of the loss function, in case the constraint has one: const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); - error_line_->setColor( - loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b, - loss_error_line_color.a); + error_line_->setColor(loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b, + loss_error_line_color.a); } void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(const float alpha) { - static const auto & default_x_color_ = relative_pose_axes_->getDefaultXColor(); - static const auto & default_y_color_ = relative_pose_axes_->getDefaultYColor(); - static const auto & default_z_color_ = relative_pose_axes_->getDefaultZColor(); + static const auto& default_x_color_ = relative_pose_axes_->getDefaultXColor(); + static const auto& default_y_color_ = relative_pose_axes_->getDefaultYColor(); + static const auto& default_z_color_ = relative_pose_axes_->getDefaultZColor(); relative_pose_axes_->setXColor( - Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT relative_pose_axes_->setYColor( - Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, alpha)); // NOLINT relative_pose_axes_->setZColor( - Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, alpha)); // NOLINT } -void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(const Ogre::Vector3 & scale) +void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(const Ogre::Vector3& scale) { relative_pose_axes_->setScale(scale); } -void RelativePose2DStampedConstraintVisual::setTextScale(const Ogre::Vector3 & scale) +void RelativePose2DStampedConstraintVisual::setTextScale(const Ogre::Vector3& scale) { text_node_->setScale(scale); } @@ -325,32 +315,32 @@ void RelativePose2DStampedConstraintVisual::setVisible(const bool visible) relative_pose_axes_node_->setVisible(visible); } -const Ogre::Vector3 & RelativePose2DStampedConstraintVisual::getPosition() +const Ogre::Vector3& RelativePose2DStampedConstraintVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion & RelativePose2DStampedConstraintVisual::getOrientation() +const Ogre::Quaternion& RelativePose2DStampedConstraintVisual::getOrientation() { return root_node_->getOrientation(); } -void RelativePose2DStampedConstraintVisual::setPosition(const Ogre::Vector3 & position) +void RelativePose2DStampedConstraintVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternion & orientation) +void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } -Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor( - const Ogre::ColourValue & color, - const float loss_scale) +Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor(const Ogre::ColourValue& color, + const float loss_scale) { // Skip if the loss scale is negative, which means the constraint has no loss: - if (loss_scale < 0.0) { + if (loss_scale < 0.0) + { return color; } @@ -373,9 +363,7 @@ Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineCol Ogre::ColourValue loss_error_line_color; loss_error_line_color.setHSB(hue, saturation, loss_brightness); - return Ogre::ColourValue( - loss_error_line_color.r, loss_error_line_color.g, - loss_error_line_color.b, color.a); + return Ogre::ColourValue(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, color.a); } } // namespace fuse_viz diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index 134438b54..4f6cac411 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -63,24 +63,21 @@ using rviz_common::properties::BoolProperty; SerializedGraphDisplay::SerializedGraphDisplay() { show_variables_property_ = - new BoolProperty( - "Variables", true, "The list of all variables.", this, - SLOT(updateShowVariables())); - - variable_property_ = new Pose2DStampedProperty( - "pose_2d", true, - "Pose2DStamped (fuse_variables::Position2DStamped + " - "fuse_variables::Orientation2DStamped) variable.", - show_variables_property_, SLOT(queueRender()), this); - - show_constraints_property_ = new BoolProperty( - "Constraints", true, "The list of all constraints by source.", this, - SLOT(updateShowConstraints())); + new BoolProperty("Variables", true, "The list of all variables.", this, SLOT(updateShowVariables())); + + variable_property_ = new Pose2DStampedProperty("pose_2d", true, + "Pose2DStamped (fuse_variables::Position2DStamped + " + "fuse_variables::Orientation2DStamped) variable.", + show_variables_property_, SLOT(queueRender()), this); + + show_constraints_property_ = new BoolProperty("Constraints", true, "The list of all constraints by source.", this, + SLOT(updateShowConstraints())); } SerializedGraphDisplay::~SerializedGraphDisplay() { - if (initialized()) { + if (initialized()) + { clear(); root_node_->removeAndDestroyAllChildren(); @@ -108,7 +105,8 @@ void SerializedGraphDisplay::onEnable() variable_property_->updateVisibility(); - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { entry.second->updateVisibility(); } } @@ -120,7 +118,7 @@ void SerializedGraphDisplay::onDisable() root_node_->setVisible(false); } -void SerializedGraphDisplay::load(const rviz_common::Config & config) +void SerializedGraphDisplay::load(const rviz_common::Config& config) { MFDClass::load(config); @@ -129,8 +127,7 @@ void SerializedGraphDisplay::load(const rviz_common::Config & config) // is present in the graph: const auto constraints_config = config.mapGetChild("Constraints"); - for (rviz_common::Config::MapIterator iter = constraints_config.mapIterator(); iter.isValid(); - iter.advance()) + for (rviz_common::Config::MapIterator iter = constraints_config.mapIterator(); iter.isValid(); iter.advance()) { constraint_source_configs_[iter.currentKey().toStdString()] = iter.currentChild(); } @@ -145,7 +142,8 @@ void SerializedGraphDisplay::updateShowConstraints() { const auto visible = show_constraints_property_->getBool(); - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { entry.second->setBool(visible); } } @@ -156,7 +154,8 @@ void SerializedGraphDisplay::clear() delete variable_property_; - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { delete entry.second; } constraint_source_properties_.clear(); @@ -169,59 +168,62 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con { Ogre::Vector3 position; Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("fuse"), - "Error transforming from frame '" << msg->header.frame_id << "' to frame '" - << qPrintable(fixed_frame_) << "'"); + if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("fuse"), "Error transforming from frame '" + << msg->header.frame_id << "' to frame '" + << qPrintable(fixed_frame_) << "'"); } root_node_->setPosition(position); root_node_->setOrientation(orientation); - for (auto & entry : variables_changed_map_) { + for (auto& entry : variables_changed_map_) + { entry.second = false; } - for (auto & entry : constraints_changed_map_) { + for (auto& entry : constraints_changed_map_) + { entry.second = false; } const auto graph = graph_deserializer_.deserialize(msg); - for (const auto & variable : graph->getVariables()) { - const auto orientation = dynamic_cast(&variable); - if (!orientation) { + for (const auto& variable : graph->getVariables()) + { + const auto orientation = dynamic_cast(&variable); + if (!orientation) + { continue; } - const auto position_uuid = fuse_variables::Position2DStamped( - orientation->stamp(), orientation->deviceId()).uuid(); - if (!graph->variableExists(position_uuid)) { + const auto position_uuid = fuse_variables::Position2DStamped(orientation->stamp(), orientation->deviceId()).uuid(); + if (!graph->variableExists(position_uuid)) + { continue; } - const auto position = - dynamic_cast(&graph->getVariable(position_uuid)); + const auto position = dynamic_cast(&graph->getVariable(position_uuid)); - variable_property_->createAndInsertOrUpdateVisual( - scene_manager_, root_node_, *position, - *orientation); + variable_property_->createAndInsertOrUpdateVisual(scene_manager_, root_node_, *position, *orientation); variables_changed_map_[position_uuid] = true; } - for (const auto & constraint : graph->getConstraints()) { - const auto relative_pose = - dynamic_cast(&constraint); - if (!relative_pose) { + for (const auto& constraint : graph->getConstraints()) + { + const auto relative_pose = dynamic_cast(&constraint); + if (!relative_pose) + { continue; } const auto constraint_uuid = constraint.uuid(); - const auto & constraint_source = constraint.source(); + const auto& constraint_source = constraint.source(); - if (source_color_map_.find(constraint_source) == source_color_map_.end()) { + if (source_color_map_.find(constraint_source) == source_color_map_.end()) + { // Generate hue color automatically based on the number of sources including the new one (n) // The hue is computed in such a way that the (dynamic) colormap is always well spread along // the spectrum. This is achieved by traversing a virtual complete binary tree in breadth- @@ -232,78 +234,90 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con const auto m = n + 1 - std::pow(2, level); const auto hue = (2 * (m - 1) + 1) / std::pow(2, level + 1); - auto & source_color = source_color_map_[constraint_source]; + auto& source_color = source_color_map_[constraint_source]; source_color.setHSB(hue, 1.0, 1.0); // Insert constraint sorted alphabetically: const auto description = constraint_source + ' ' + constraint.type() + " constraint."; - const auto constraint_source_property = new RelativePose2DStampedConstraintProperty( - QString::fromStdString(constraint_source), true, QString::fromStdString( - description), nullptr, - SLOT(queueRender()), this); + const auto constraint_source_property = + new RelativePose2DStampedConstraintProperty(QString::fromStdString(constraint_source), true, + QString::fromStdString(description), nullptr, SLOT(queueRender()), + this); const auto result = constraint_source_properties_.insert( - {constraint_source, constraint_source_property}); // NOLINT(whitespace/braces) + { constraint_source, constraint_source_property }); // NOLINT(whitespace/braces) - if (!result.second) { + if (!result.second) + { delete constraint_source_property; throw std::runtime_error("Failed to insert " + description); } - show_constraints_property_->addChild( - constraint_source_property, - std::distance(constraint_source_properties_.begin(), result.first)); - - if (constraint_source_configs_.find(constraint_source) == constraint_source_configs_.end()) { - constraint_source_properties_[constraint_source]->setColor( - rviz_common::properties::ogreToQt( - source_color)); - } else { - constraint_source_properties_[constraint_source]->load( - constraint_source_configs_[ - constraint_source]); + show_constraints_property_->addChild(constraint_source_property, + std::distance(constraint_source_properties_.begin(), result.first)); + + if (constraint_source_configs_.find(constraint_source) == constraint_source_configs_.end()) + { + constraint_source_properties_[constraint_source]->setColor(rviz_common::properties::ogreToQt(source_color)); + } + else + { + constraint_source_properties_[constraint_source]->load(constraint_source_configs_[constraint_source]); } } - if (constraint_visuals_.find(constraint_uuid) == constraint_visuals_.end()) { - constraint_visuals_[constraint_uuid] = - constraint_source_properties_[constraint_source]->createAndInsertVisual( - scene_manager_, root_node_, *relative_pose, *graph); - } else { + if (constraint_visuals_.find(constraint_uuid) == constraint_visuals_.end()) + { + constraint_visuals_[constraint_uuid] = constraint_source_properties_[constraint_source]->createAndInsertVisual( + scene_manager_, root_node_, *relative_pose, *graph); + } + else + { constraint_visuals_[constraint_uuid]->setConstraint(*relative_pose, *graph); } constraints_changed_map_[constraint_uuid] = true; } - for (const auto & entry : variables_changed_map_) { - if (!entry.second) { + for (const auto& entry : variables_changed_map_) + { + if (!entry.second) + { variable_property_->eraseVisual(entry.first); } } - for (auto it = variables_changed_map_.begin(); it != variables_changed_map_.end(); ) { - if (it->second) { + for (auto it = variables_changed_map_.begin(); it != variables_changed_map_.end();) + { + if (it->second) + { ++it; - } else { + } + else + { it = variables_changed_map_.erase(it); } } - for (const auto & entry : constraints_changed_map_) { - if (!entry.second) { - constraint_source_properties_[constraint_visuals_[entry.first]->getSource()]->eraseVisual( - entry.first); + for (const auto& entry : constraints_changed_map_) + { + if (!entry.second) + { + constraint_source_properties_[constraint_visuals_[entry.first]->getSource()]->eraseVisual(entry.first); constraint_visuals_.erase(entry.first); } } - for (auto it = constraints_changed_map_.begin(); it != constraints_changed_map_.end(); ) { - if (it->second) { + for (auto it = constraints_changed_map_.begin(); it != constraints_changed_map_.end();) + { + if (it->second) + { ++it; - } else { + } + else + { it = constraints_changed_map_.erase(it); } }