diff --git a/beluga/docs/doxygen_cites.bib b/beluga/docs/doxygen_cites.bib index d04b9f440..9dd1549d5 100644 --- a/beluga/docs/doxygen_cites.bib +++ b/beluga/docs/doxygen_cites.bib @@ -9,6 +9,16 @@ @book{thrun2005probabilistic publisher={MIT Press} } +@book{gentle2009computationalstatistics, + title={Computational Statistics}, + author={Gentle, J. E.}, + isbn={9780387981437}, + series={Statistics and Computing}, + year={2009}, + publisher={Springer}, + doi={10.1007/978-0-387-98144-4} +} + @article{grisetti2007selectiveresampling, author={Grisetti, Giorgio and Stachniss, Cyrill and Burgard, Wolfram}, journal={IEEE Transactions on Robotics}, diff --git a/beluga/include/beluga/random/multivariate_distribution_traits.hpp b/beluga/include/beluga/random/multivariate_distribution_traits.hpp new file mode 100644 index 000000000..eebab20d2 --- /dev/null +++ b/beluga/include/beluga/random/multivariate_distribution_traits.hpp @@ -0,0 +1,115 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RANDOM_MULTIVARIATE_DISTRIBUTION_TRAITS_HPP +#define BELUGA_RANDOM_MULTIVARIATE_DISTRIBUTION_TRAITS_HPP + +#include +#include + +/** + * \file + * \brief Implementation of multivariate distribution traits. + */ + +namespace beluga { + +/// Forward declaration of the multivariate_distribution_traits class template. +template +struct multivariate_distribution_traits; + +/// Specialization for types derived from `Eigen::EigenBase`. +template +struct multivariate_distribution_traits, T>>> { + static_assert(T::ColsAtCompileTime == 1 || T::RowsAtCompileTime == 1, "T should be a column or row vector"); + + /// Extract size information and types from the Eigen matrix type T. + static constexpr int matrix_size = T::ColsAtCompileTime > T::RowsAtCompileTime // + ? T::ColsAtCompileTime + : T::RowsAtCompileTime; + + /// The scalar type. + using scalar_type = typename T::Scalar; + + /// The result type representation. + using result_type = typename T::PlainMatrix; + + /// The vector type. + using vector_type = typename T::PlainMatrix; + + /// The covariance matrix type. + using covariance_type = typename Eigen::Matrix; + + /// Convert from result to vector representation. + [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { return t; } + + /// Convert from vector to result representation. + [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) { return v; } +}; + +/// Specialization for types derived from Sophus::SO2Base. +template +struct multivariate_distribution_traits, T>>> { + /// The scalar type. + using scalar_type = typename T::Scalar; + + /// The result type representation. + using result_type = Sophus::SO2; + + /// The vector type. + using vector_type = typename Eigen::Matrix; + + /// The covariance matrix type. + using covariance_type = typename Eigen::Matrix; + + /// Convert from result to vector representation. + [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { return vector_type{t.log()}; } + + /// Convert from vector to result representation. + [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) { + return Sophus::SO2::exp(v.x()); + } +}; + +/// Specialization for types derived from Sophus::SE2Base. +template +struct multivariate_distribution_traits, T>>> { + /// The scalar type. + using scalar_type = typename T::Scalar; + + /// The result type representation. + using result_type = Sophus::SE2; + + /// The vector type. + using vector_type = typename Eigen::Matrix; + + /// The covariance matrix type. + using covariance_type = typename Eigen::Matrix; + + /// Convert from result to vector representation. + [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { + vector_type v; + v << t.translation(), t.so2().log(); + return v; + } + + /// Convert from vector to result representation. + [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) { + return {Sophus::SO2::exp(v.z()), v.head(2)}; + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/random/multivariate_normal_distribution.hpp b/beluga/include/beluga/random/multivariate_normal_distribution.hpp index 269b23f33..a10195aed 100644 --- a/beluga/include/beluga/random/multivariate_normal_distribution.hpp +++ b/beluga/include/beluga/random/multivariate_normal_distribution.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Ekumen, Inc. +// Copyright 2023-2024 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,11 +16,10 @@ #define BELUGA_RANDOM_MULTIVARIATE_NORMAL_DISTRIBUTION_HPP #include - -#include -#include #include +#include + /** * \file * \brief Implementation of a multivariate normal distribution. @@ -28,135 +27,201 @@ namespace beluga { +/// Multivariate normal distribution parameter set class. +template +class MultivariateNormalDistributionParam { + public: + static_assert(std::is_base_of_v, Vector>, "Vector should be an Eigen type"); + static_assert( + Vector::ColsAtCompileTime == 1 || Vector::RowsAtCompileTime == 1, + "Vector should be a column or row vector"); + + /// The scalar type. + using scalar_type = typename Vector::Scalar; + + /// The vector type. + using vector_type = Vector; + + /// The covariance matrix from Vector. + using matrix_type = Matrix; + + /// Constructs a parameter set instance. + MultivariateNormalDistributionParam() = default; + + /// Constructs a parameter set instance. + /** + * \param covariance Real symmetric matrix that represents the covariance of the random variable. + * + * \throw std::runtime_error If the provided covariance is invalid. + */ + explicit MultivariateNormalDistributionParam(matrix_type covariance) + : transform_{make_transform(std::move(covariance))} {} + + /// Constructs a parameter set instance. + /** + * \param mean A vector that represents the mean value of the random variable. + * \param covariance Real symmetric matrix that represents the covariance of the random variable. + * + * \throw std::runtime_error If the provided covariance is invalid. + */ + MultivariateNormalDistributionParam(vector_type mean, matrix_type covariance) + : mean_{std::move(mean)}, transform_{make_transform(std::move(covariance))} {} + + /// Compares this object with other parameter set object. + /** + * \param other Parameter set object to compare against. + * \return True if the objects are equal, false otherwise. + */ + [[nodiscard]] bool operator==(const MultivariateNormalDistributionParam& other) const { + return mean_ == other.mean_ && transform_ == other.transform_; + } + + /// Compares this object with other parameter set object. + /** + * \param other Parameter set object to compare against. + * \return True if the objects are not equal, false otherwise. + */ + [[nodiscard]] bool operator!=(const MultivariateNormalDistributionParam& other) const { return !(*this == other); } + + /// Generates a new random object from the distribution. + /** + * \tparam Generator The generator type that must meet the requirements of + * [UniformRandomBitGenerator](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator). + * \param distribution A reference to a standard normal distribution instance. + * \param generator An uniform random bit generator object. + * \return The generated random object. + */ + template + [[nodiscard]] auto operator()(std::normal_distribution& distribution, Generator& generator) const { + const auto delta = vector_type{}.unaryExpr([&distribution, &generator](auto) { return distribution(generator); }); + if constexpr (vector_type::ColsAtCompileTime == 1) { + return mean_ + transform_ * delta; + } else { + return mean_ + delta * transform_; + } + } + + private: + vector_type mean_{vector_type::Zero()}; + matrix_type transform_{make_transform(vector_type::Ones().asDiagonal())}; + + [[nodiscard]] static matrix_type make_transform(matrix_type covariance) { + // For more information about the method used to generate correlated normal vectors + // from independent normal distributions, see: + // https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution + // \cite gentle2009computationalstatistics. + if (!covariance.isApprox(covariance.transpose())) { + throw std::runtime_error("Invalid covariance matrix, it is not symmetric."); + } + const auto solver = Eigen::SelfAdjointEigenSolver{covariance}; + if (solver.info() != Eigen::Success) { + throw std::runtime_error("Invalid covariance matrix, eigen solver failed."); + } + const auto& eigenvalues = solver.eigenvalues(); + if ((eigenvalues.array() < 0.0).any()) { + throw std::runtime_error("Invalid covariance matrix, it has negative eigenvalues."); + } + return solver.eigenvectors() * eigenvalues.cwiseSqrt().asDiagonal(); + } +}; + /// Multivariate normal distribution. /** - * `MultivariateNormalDistribution` is an implementation of + * `MultivariateNormalDistribution` is an implementation of * \ref RandomStateDistributionPage that represents a multi-dimensional * normal distribution of real-valued random variables. * - * \tparam Matrix The type of the covariance matrix; this is expected to be an instantiation - * of the [Eigen::Matrix](https://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html) class - * template. + * \tparam T The result type for the distribution. */ -template +template class MultivariateNormalDistribution { public: - /// Scalar type for matrices of type Matrix. - using Scalar = typename Matrix::Scalar; - - /// The eigen column vector from Matrix. - using Vector = typename Eigen::Matrix; - - /// Multivariate normal distribution parameter set class. - class Param { - public: - /// The type of distribution to which this parameter set class belongs. - using distribution_type = MultivariateNormalDistribution; - - /// Constructs a parameter set instance. - Param() = default; - - /// Constructs a parameter set instance. - /** - * \tparam InputType The input type derived from `EigenBase`. - * \param covariance Real symmetric matrix that represents the covariance of the random variable. - * - * \throw std::runtime_error If the provided covariance is invalid. - */ - template - explicit Param(const Eigen::EigenBase& covariance) : transform_{make_transform(covariance)} {} - - /// Constructs a parameter set instance. - /** - * \tparam InputType The input type derived from `EigenBase`. - * \param mean A vector that represents the mean value of the random variable. - * \param covariance Real symmetric matrix that represents the covariance of the random variable. - * - * \throw std::runtime_error If the provided covariance is invalid. - */ - template - Param(Vector mean, const Eigen::EigenBase& covariance) - : mean_{std::move(mean)}, transform_{make_transform(covariance)} {} - - /// Compares this object with other parameter set object. - /** - * \param other Parameter set object to compare against. - * \return True if the objects are equal, false otherwise. - */ - [[nodiscard]] bool operator==(const Param& other) const { - return mean_ == other.mean_ && transform_ == other.transform_; - } + template + friend class MultivariateNormalDistribution; - /// Compares this object with other parameter set object. - /** - * \param other Parameter set object to compare against. - * \return True if the objects are not equal, false otherwise. - */ - [[nodiscard]] bool operator!=(const Param& other) const { return !(*this == other); } - - private: - friend class MultivariateNormalDistribution; - - Vector mean_{Vector::Zero()}; - Matrix transform_{make_transform(Vector::Ones().asDiagonal())}; - - [[nodiscard]] static Matrix make_transform(const Matrix& covariance) { - // For more information about the method used to generate correlated normal vectors - // from independent normal distributions, see: - // https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution - // Gentle, J. E. (2009). Computational Statistics. Statistics and Computing. New York: Springer. pp. 315–316. - // https://doi.org/10.1007%2F978-0-387-98144-4 - if (!covariance.isApprox(covariance.transpose())) { - throw std::runtime_error("Invalid covariance matrix, it is not symmetric."); - } - const auto solver = Eigen::SelfAdjointEigenSolver{covariance}; - if (solver.info() != Eigen::Success) { - throw std::runtime_error("Invalid covariance matrix, eigen solver failed."); - } - const auto& eigenvalues = solver.eigenvalues(); - if ((eigenvalues.array() < 0.0).any()) { - throw std::runtime_error("Invalid covariance matrix, it has negative eigenvalues."); - } - return solver.eigenvectors() * eigenvalues.cwiseSqrt().asDiagonal(); - } - }; + /// The scalar type. + using scalar_type = typename multivariate_distribution_traits::scalar_type; - /// The type of the parameter set. - using param_type = Param; + /// The Eigen vector type corresponding to T. + using vector_type = typename multivariate_distribution_traits::vector_type; + + /// The covariance matrix from T. + using covariance_type = typename multivariate_distribution_traits::covariance_type; + + /// The result type from T. + using result_type = typename multivariate_distribution_traits::result_type; - /// The result type generated by the generator. - using result_type = Vector; + /// The type of the parameter set. + using param_type = MultivariateNormalDistributionParam; - /// Constructs a MultivariateNormalDistribution with zero mean and circularly symmetric unitary covariance. + /// Construct with zero mean and circularly symmetric unitary covariance. MultivariateNormalDistribution() = default; - /// Constructs a MultivariateNormalDistribution using param as distribution parameters. + /// Construct from distribution parameters. /** * \param params The distribution parameter set. */ explicit MultivariateNormalDistribution(const param_type& params) : params_{params} {} - /// Constructs a MultivariateNormalDistribution with zero mean and the given covariance. + /// Construct with zero mean and the given covariance. /** - * \tparam InputType The input type derived from `EigenBase`. * \param covariance Real symmetric matrix that represents the covariance of the random variable. * * \throw std::runtime_error If the provided covariance is invalid. */ - template - explicit MultivariateNormalDistribution(const Eigen::EigenBase& covariance) : params_{covariance} {} + explicit MultivariateNormalDistribution(covariance_type covariance) : params_{std::move(covariance)} {} - /// Constructs a MultivariateNormalDistribution with the given mean and covariance. + /// Construct with the given mean and covariance. /** - * \tparam InputType The input type derived from `EigenBase`. - * \param mean A vector that represents the mean value of the random variable. + * \param mean An object that represents the mean value of the random variable. * \param covariance Real symmetric matrix that represents the covariance of the random variable. * * \throw std::runtime_error If the provided covariance is invalid. */ - template - MultivariateNormalDistribution(const Vector& mean, const Eigen::EigenBase& covariance) - : params_(mean, covariance) {} + MultivariateNormalDistribution(result_type mean, covariance_type covariance) + : params_{multivariate_distribution_traits::to_vector(std::move(mean)), std::move(covariance)} {} + + /// Copy construct from another compatible distribution. + /** + * \tparam U The result type for the other distribution. + * \param other Another instance of a multivariate normal distribution. + */ + template + /* implicit */ MultivariateNormalDistribution(const MultivariateNormalDistribution& other) // NOLINT + : params_{other.params_}, distribution_{other.distribution_} {} + + /// Move construct from another compatible distribution. + /** + * \tparam U The result type for the other distribution. + * \param other Another instance of a multivariate normal distribution. + */ + template + /* implicit */ MultivariateNormalDistribution(MultivariateNormalDistribution&& other) noexcept // NOLINT + : params_(std::move(other.params_)), distribution_{std::move(other.distribution_)} {} + + /// Copy assign from another compatible distribution. + /** + * \tparam U The result type for the other distribution. + * \param other Another instance of a multivariate normal distribution. + */ + template + MultivariateNormalDistribution& operator=(const MultivariateNormalDistribution& other) { + params_ = other.params_; + distribution_ = other.distribution_; + return *this; + } + + /// Move assign from another compatible distribution. + /** + * \tparam U The result type for the other distribution. + * \param other Another instance of a multivariate normal distribution. + */ + template + MultivariateNormalDistribution& operator=(MultivariateNormalDistribution&& other) { + params_ = std::move(other.params_); + distribution_ = std::move(other.distribution_); + return *this; + } /// Resets the internal state of the distribution. void reset() { distribution_.reset(); } @@ -181,7 +246,7 @@ class MultivariateNormalDistribution { */ template [[nodiscard]] result_type operator()(Generator& generator) { - return this->operator()(generator, params_); + return (*this)(generator, params_); } /// Generates the next random object in the distribution. @@ -194,8 +259,7 @@ class MultivariateNormalDistribution { */ template [[nodiscard]] result_type operator()(Generator& generator, const param_type& params) { - return params.mean_ + - params.transform_ * Vector{}.unaryExpr([this, &generator](auto) { return distribution_(generator); }); + return multivariate_distribution_traits::from_vector(params(distribution_, generator)); } /// Compares this object with other distribution object. @@ -206,7 +270,7 @@ class MultivariateNormalDistribution { * \param other Distribution object to compare against. * \return True if the objects are equal, false otherwise. */ - [[nodiscard]] bool operator==(const MultivariateNormalDistribution& other) const { + [[nodiscard]] bool operator==(const MultivariateNormalDistribution& other) const { return params_ == other.params_ && distribution_ == other.distribution_; } @@ -218,22 +282,17 @@ class MultivariateNormalDistribution { * \param other Distribution object to compare against. * \return True if the objects are not equal, false otherwise. */ - [[nodiscard]] bool operator!=(const MultivariateNormalDistribution& other) const { return !(*this == other); } + [[nodiscard]] bool operator!=(const MultivariateNormalDistribution& other) const { return !(*this == other); } private: param_type params_; - std::normal_distribution distribution_; + std::normal_distribution distribution_; }; -/// Deduction guide to help CTAD deduce the correct Eigen type. -template -MultivariateNormalDistribution(const Eigen::EigenBase&) - -> MultivariateNormalDistribution; - -/// Deduction guide to help CTAD deduce the correct Eigen type. -template -MultivariateNormalDistribution(const VectorType&, const Eigen::EigenBase&) - -> MultivariateNormalDistribution; +/// Deduction guide to deduce the correct result type. +template +MultivariateNormalDistribution(const T&, const typename multivariate_distribution_traits::covariance_type&) + -> MultivariateNormalDistribution::result_type>; } // namespace beluga diff --git a/beluga/test/beluga/random/test_multivariate_normal_distribution.cpp b/beluga/test/beluga/random/test_multivariate_normal_distribution.cpp index 900a11081..18f2bb62c 100644 --- a/beluga/test/beluga/random/test_multivariate_normal_distribution.cpp +++ b/beluga/test/beluga/random/test_multivariate_normal_distribution.cpp @@ -27,7 +27,7 @@ namespace { TEST(MultivariateNormalDistribution, CopyAndCompare) { Eigen::Matrix3d covariance = Eigen::Vector3d::Ones().asDiagonal(); - auto distribution = beluga::MultivariateNormalDistribution{covariance}; + auto distribution = beluga::MultivariateNormalDistribution{covariance}; auto other_distribution = distribution; ASSERT_EQ(distribution, other_distribution); auto generator = std::mt19937{std::random_device()()}; @@ -39,12 +39,12 @@ TEST(MultivariateNormalDistribution, CopyAndCompare) { TEST(MultivariateNormalDistribution, NegativeEigenvaluesMatrix) { Eigen::Matrix3d covariance = Eigen::Matrix3d::Ones(); - EXPECT_THROW(beluga::MultivariateNormalDistribution{covariance}, std::runtime_error); + EXPECT_THROW(beluga::MultivariateNormalDistribution{covariance}, std::runtime_error); } TEST(MultivariateNormalDistribution, NonSymmetricMatrix) { auto covariance = testing::as({{1.0, 2.0, 0.0}, {1.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}); - EXPECT_THROW(beluga::MultivariateNormalDistribution{covariance}, std::runtime_error); + EXPECT_THROW(beluga::MultivariateNormalDistribution{covariance}, std::runtime_error); } TEST(MultivariateNormalDistribution, SampleZero) { @@ -86,4 +86,31 @@ INSTANTIATE_TEST_SUITE_P( std::make_pair(Eigen::Vector2d{3.0, 4.0}, testing::as({{1.5, -0.3}, {-0.3, 1.5}})), std::make_pair(Eigen::Vector2d{5.0, 6.0}, testing::as({{2.0, 0.7}, {0.7, 2.0}})))); +TEST(MultivariateNormalDistribution, RowVector) { + auto generator = std::mt19937{std::random_device()()}; + auto expected_mean = Eigen::RowVector2d{1.0, 2.0}; + auto distribution = beluga::MultivariateNormalDistribution{expected_mean, Eigen::Matrix2d::Zero()}; + auto mean = distribution(generator); + ASSERT_NEAR(mean(0), expected_mean(0), 0.001); + ASSERT_NEAR(mean(1), expected_mean(1), 0.001); +} + +TEST(MultivariateNormalDistribution, SO2Element) { + auto generator = std::mt19937{std::random_device()()}; + auto expected_mean = Sophus::SO2d{1.5}; + auto distribution = beluga::MultivariateNormalDistribution{expected_mean, Eigen::Matrix::Zero()}; + auto mean = distribution(generator); + ASSERT_NEAR((mean).log(), expected_mean.log(), 0.001); +} + +TEST(MultivariateNormalDistribution, SE2Element) { + auto generator = std::mt19937{std::random_device()()}; + auto expected_mean = Sophus::SE2d{Sophus::SO2d{1.57}, Eigen::Vector2d{1.0, 2.0}}; + auto distribution = beluga::MultivariateNormalDistribution{expected_mean, Eigen::Matrix3d::Zero()}; + auto mean = distribution(generator); + ASSERT_NEAR(mean.so2().log(), expected_mean.so2().log(), 0.001); + ASSERT_NEAR(mean.translation()(0), expected_mean.translation()(0), 0.001); + ASSERT_NEAR(mean.translation()(1), expected_mean.translation()(1), 0.001); +} + } // namespace diff --git a/beluga_system_tests/test/test_system.cpp b/beluga_system_tests/test/test_system.cpp index 3ae6609bb..6a3ae617a 100644 --- a/beluga_system_tests/test/test_system.cpp +++ b/beluga_system_tests/test/test_system.cpp @@ -74,8 +74,6 @@ using MonteCarloLocalization2d = beluga::MonteCarloLocalization2d< LaserLocalizationInterface2d, ConcreteResamplingPoliciesPoller>; -using beluga::MultivariateNormalDistribution; - struct InitialPose { Eigen::Vector3d mean; Eigen::Matrix3d covariance; @@ -112,7 +110,7 @@ TEST_P(BelugaSystemTest, testEstimatedPath) { auto test_data = GetParam()(); auto& pf = *test_data.particle_filter; pf.initialize_states( - ranges::views::generate([distribution = MultivariateNormalDistribution{ + ranges::views::generate([distribution = beluga::MultivariateNormalDistribution{ test_data.initial_pose.mean, test_data.initial_pose.covariance}]() mutable { static auto generator = std::mt19937{std::random_device()()}; const auto sample = distribution(generator); diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index 537a064fc..68e490222 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -125,19 +125,7 @@ auto particle_filter_test( auto hasher = beluga::spatial_hash{0.1, 0.1, 0.1}; - // Use the initial distribution to initialize particles. - // TODO(nahuel): We should have a view to convert from Eigen to Sophus types. - /** - * auto particles = beluga::views::sample(initial_distribution) | - * (something to convert from Eigen::Vector3d to Sophus::SE2d) | - * ranges::views::transform(beluga::make_from_state) | - * ranges::views::take_exactly(params.max_particles) | - * ranges::to; - */ - auto particles = beluga::views::sample(initial_distribution) | // - ranges::views::transform([](const auto& sample) { - return Sophus::SE2d{Sophus::SO2d{sample.z()}, Eigen::Vector2d{sample.x(), sample.y()}}; - }) | + auto particles = beluga::views::sample(initial_distribution) | // ranges::views::transform(beluga::make_from_state) | // ranges::views::take_exactly(params.max_particles) | // ranges::to; @@ -296,7 +284,7 @@ auto get_perfect_odometry_data() { auto datapoints = ranges::views::zip(measurements, odometry, ground_truth); auto initial_distribution = beluga::MultivariateNormalDistribution{ - Eigen::Vector3d{0.0, 2.0, 0.0}, // initial pose mean + Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{0.0, 2.0}}, // initial pose mean Eigen::Matrix3d{{0.125, 0.0, 0.0}, {0.0, 0.125, 0.0}, {0.0, 0.0, 0.04}} // initial pose covariance };