From d30622d35d18aa51ddcf2b56fadf69946c9a34a6 Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 17:28:50 +0800 Subject: [PATCH 01/10] Add Kalman Filter --- rmoss_util/CMakeLists.txt | 39 ++-- rmoss_util/README.md | 45 ++++- rmoss_util/common/CMakeLists.txt | 25 +++ .../{ => common}/include/rmoss_util/debug.hpp | 0 .../include/rmoss_util/image_utils.hpp | 0 .../include/rmoss_util/task_manager.hpp | 0 .../include/rmoss_util/time_utils.hpp | 0 .../include/rmoss_util/url_resolver.hpp | 0 rmoss_util/{ => common}/src/debug.cpp | 0 rmoss_util/{ => common}/src/image_utils.cpp | 0 rmoss_util/{ => common}/src/task_manager.cpp | 0 rmoss_util/{ => common}/src/time_utils.cpp | 0 rmoss_util/{ => common}/src/url_resolver.cpp | 0 rmoss_util/{ => common}/test/CMakeLists.txt | 2 +- .../{ => common}/test/test_url_resolve.cpp | 0 rmoss_util/math/CMakeLists.txt | 17 ++ .../math/include/rmoss_util/kalman_filter.hpp | 173 ++++++++++++++++++ .../include/rmoss_util/mono_measure_tool.hpp | 2 +- .../{ => math}/src/mono_measure_tool.cpp | 5 +- rmoss_util/math/test/CMakeLists.txt | 4 + rmoss_util/math/test/test_kalman_filter.cpp | 87 +++++++++ rmoss_util/package.xml | 2 + 22 files changed, 370 insertions(+), 31 deletions(-) create mode 100644 rmoss_util/common/CMakeLists.txt rename rmoss_util/{ => common}/include/rmoss_util/debug.hpp (100%) rename rmoss_util/{ => common}/include/rmoss_util/image_utils.hpp (100%) rename rmoss_util/{ => common}/include/rmoss_util/task_manager.hpp (100%) rename rmoss_util/{ => common}/include/rmoss_util/time_utils.hpp (100%) rename rmoss_util/{ => common}/include/rmoss_util/url_resolver.hpp (100%) rename rmoss_util/{ => common}/src/debug.cpp (100%) rename rmoss_util/{ => common}/src/image_utils.cpp (100%) rename rmoss_util/{ => common}/src/task_manager.cpp (100%) rename rmoss_util/{ => common}/src/time_utils.cpp (100%) rename rmoss_util/{ => common}/src/url_resolver.cpp (100%) rename rmoss_util/{ => common}/test/CMakeLists.txt (60%) rename rmoss_util/{ => common}/test/test_url_resolve.cpp (100%) create mode 100644 rmoss_util/math/CMakeLists.txt create mode 100644 rmoss_util/math/include/rmoss_util/kalman_filter.hpp rename rmoss_util/{ => math}/include/rmoss_util/mono_measure_tool.hpp (96%) rename rmoss_util/{ => math}/src/mono_measure_tool.cpp (97%) create mode 100644 rmoss_util/math/test/CMakeLists.txt create mode 100644 rmoss_util/math/test/test_kalman_filter.cpp diff --git a/rmoss_util/CMakeLists.txt b/rmoss_util/CMakeLists.txt index f29074e..638283a 100644 --- a/rmoss_util/CMakeLists.txt +++ b/rmoss_util/CMakeLists.txt @@ -10,39 +10,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find package find_package(ament_cmake REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(rmoss_interfaces REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(OpenCV REQUIRED) +add_subdirectory(common) +add_subdirectory(math) -# include -include_directories(include) - -# create rmoss_tool lib -aux_source_directory(${PROJECT_SOURCE_DIR}/src DIR_SRCS) -add_library(${PROJECT_NAME} SHARED ${DIR_SRCS}) -ament_target_dependencies(${PROJECT_NAME} - rclcpp - ament_index_cpp - rcpputils - sensor_msgs - rmoss_interfaces - cv_bridge - OpenCV -) +add_library(${PROJECT_NAME}::common ALIAS common) +add_library(${PROJECT_NAME}::math ALIAS math) # Install include directories -install(DIRECTORY include/ +install(DIRECTORY common/include/ math/include/ DESTINATION include ) # Install libraries -install(TARGETS ${PROJECT_NAME} +install(TARGETS common math EXPORT ${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -57,12 +38,18 @@ ament_export_dependencies(sensor_msgs) ament_export_dependencies(image_transport) ament_export_dependencies(cv_bridge) ament_export_dependencies(OpenCV) +ament_export_dependencies(rmoss_interfaces) +ament_export_dependencies(rcpputils) +ament_export_dependencies(ament_index_cpp) +ament_export_dependencies(Eigen3) +ament_export_dependencies(Ceres) # test if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - add_subdirectory(test) + add_subdirectory(common/test) + add_subdirectory(math/test) endif() ament_package() \ No newline at end of file diff --git a/rmoss_util/README.md b/rmoss_util/README.md index 0eae2da..4b53ee1 100644 --- a/rmoss_util/README.md +++ b/rmoss_util/README.md @@ -14,6 +14,7 @@ rmoss_util是rmoss_core 中的一个公共基础包，提供一些公共基础 * `time_utils.hpp/cpp`（不建议使用，开发中） : 时间工具，用于测量运行时间。 * `mono_measure_tool.hpp/cpp`（不建议使用，开发中） : 单目测量工具类，单目算法封装（PNP解算，相似三角形反投影等） * `url_resolver.hpp/cpp` : URL 解析器，用于解析类似 camera_info_manager 的 URL，便于灵活路径管理 +* `kalman_filter.hpp/cpp`:卡尔曼滤波器，利用Ceres的自动微分功能实现雅克比矩阵的自动求解 ## 快速使用 @@ -104,4 +105,46 @@ std::result = rmoss_util::URLResolver::getResolvedPath(url); // result = " operator()( + const Eigen::Matrix & x) const + { + Eigen::Matrix x_new; + x_new[0] = x[0] + x[1] * dt; + x_new[1] = x[1]; + return x_new; + } + + double dt; +}; + +struct Measure { + Eigen::Matrix operator()( + const Eigen::Matrix & x) const + { + Eigen::Matrix z; + z[0] = x[0]; + return z; + } +}; + +// x: 初始状态, P: 初始协方差矩阵 +rmoss_util::KalmanFilter<2> ekf(x, P); + +for (int i = 0; i < 10; i++) { + // 预测步 + ekf.predict(Predict(0.05), Q); // Q: 过程噪声 + // 更新步 + Eigen::Vector2d x_p = ekf.update(Measure(), R, z); // R: 观测噪声, z: 观测量 +} +``` + diff --git a/rmoss_util/common/CMakeLists.txt b/rmoss_util/common/CMakeLists.txt new file mode 100644 index 0000000..ddb5493 --- /dev/null +++ b/rmoss_util/common/CMakeLists.txt @@ -0,0 +1,25 @@ +# find package +find_package(ament_index_cpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rmoss_interfaces REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) + +# create rmoss_util::common lib +aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR}/src COMMON_SRCS) +add_library(common SHARED ${COMMON_SRCS}) +ament_target_dependencies(common + rclcpp + ament_index_cpp + rcpputils + sensor_msgs + rmoss_interfaces + cv_bridge + OpenCV +) +target_include_directories(common PUBLIC + $ + $ +) diff --git a/rmoss_util/include/rmoss_util/debug.hpp b/rmoss_util/common/include/rmoss_util/debug.hpp similarity index 100% rename from rmoss_util/include/rmoss_util/debug.hpp rename to rmoss_util/common/include/rmoss_util/debug.hpp diff --git a/rmoss_util/include/rmoss_util/image_utils.hpp b/rmoss_util/common/include/rmoss_util/image_utils.hpp similarity index 100% rename from rmoss_util/include/rmoss_util/image_utils.hpp rename to rmoss_util/common/include/rmoss_util/image_utils.hpp diff --git a/rmoss_util/include/rmoss_util/task_manager.hpp b/rmoss_util/common/include/rmoss_util/task_manager.hpp similarity index 100% rename from rmoss_util/include/rmoss_util/task_manager.hpp rename to rmoss_util/common/include/rmoss_util/task_manager.hpp diff --git a/rmoss_util/include/rmoss_util/time_utils.hpp b/rmoss_util/common/include/rmoss_util/time_utils.hpp similarity index 100% rename from rmoss_util/include/rmoss_util/time_utils.hpp rename to rmoss_util/common/include/rmoss_util/time_utils.hpp diff --git a/rmoss_util/include/rmoss_util/url_resolver.hpp b/rmoss_util/common/include/rmoss_util/url_resolver.hpp similarity index 100% rename from rmoss_util/include/rmoss_util/url_resolver.hpp rename to rmoss_util/common/include/rmoss_util/url_resolver.hpp diff --git a/rmoss_util/src/debug.cpp b/rmoss_util/common/src/debug.cpp similarity index 100% rename from rmoss_util/src/debug.cpp rename to rmoss_util/common/src/debug.cpp diff --git a/rmoss_util/src/image_utils.cpp b/rmoss_util/common/src/image_utils.cpp similarity index 100% rename from rmoss_util/src/image_utils.cpp rename to rmoss_util/common/src/image_utils.cpp diff --git a/rmoss_util/src/task_manager.cpp b/rmoss_util/common/src/task_manager.cpp similarity index 100% rename from rmoss_util/src/task_manager.cpp rename to rmoss_util/common/src/task_manager.cpp diff --git a/rmoss_util/src/time_utils.cpp b/rmoss_util/common/src/time_utils.cpp similarity index 100% rename from rmoss_util/src/time_utils.cpp rename to rmoss_util/common/src/time_utils.cpp diff --git a/rmoss_util/src/url_resolver.cpp b/rmoss_util/common/src/url_resolver.cpp similarity index 100% rename from rmoss_util/src/url_resolver.cpp rename to rmoss_util/common/src/url_resolver.cpp diff --git a/rmoss_util/test/CMakeLists.txt b/rmoss_util/common/test/CMakeLists.txt similarity index 60% rename from rmoss_util/test/CMakeLists.txt rename to rmoss_util/common/test/CMakeLists.txt index a317c3c..21efdc0 100644 --- a/rmoss_util/test/CMakeLists.txt +++ b/rmoss_util/common/test/CMakeLists.txt @@ -1,4 +1,4 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_url_resolve test_url_resolve.cpp) -target_link_libraries(test_url_resolve ${PROJECT_NAME}) \ No newline at end of file +target_link_libraries(test_url_resolve ${PROJECT_NAME}::common) \ No newline at end of file diff --git a/rmoss_util/test/test_url_resolve.cpp b/rmoss_util/common/test/test_url_resolve.cpp similarity index 100% rename from rmoss_util/test/test_url_resolve.cpp rename to rmoss_util/common/test/test_url_resolve.cpp diff --git a/rmoss_util/math/CMakeLists.txt b/rmoss_util/math/CMakeLists.txt new file mode 100644 index 0000000..4e8586e --- /dev/null +++ b/rmoss_util/math/CMakeLists.txt @@ -0,0 +1,17 @@ +# find package +find_package(Eigen3 REQUIRED) +find_package(Ceres REQUIRED) +find_package(OpenCV REQUIRED) + +# create rmoss_util::math lib +aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR}/src MATH_SRCS) +add_library(math SHARED ${MATH_SRCS}) +ament_target_dependencies(math + Eigen3 + Ceres + OpenCV +) +target_include_directories(math PUBLIC + $ + $ +) diff --git a/rmoss_util/math/include/rmoss_util/kalman_filter.hpp b/rmoss_util/math/include/rmoss_util/kalman_filter.hpp new file mode 100644 index 0000000..39a6eb6 --- /dev/null +++ b/rmoss_util/math/include/rmoss_util/kalman_filter.hpp @@ -0,0 +1,173 @@ +// Copyright 2024 RoboMaster-OSS +// +// 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 RMOSS_UTIL__KALMAN_FILTER_HPP_ +#define RMOSS_UTIL__KALMAN_FILTER_HPP_ + +#include +#include + +// 一些Eigen的工具函数，参考https://github.com/TomLKoller/ADEKF +namespace Eigen +{ +// 获取一个Eigen矩阵的行数 +template +static constexpr int dof = + internal::traits::RowsAtCompileTime; + +// 用于绕过Eigen表达式模板，获取表达式结果。如果不使用eval()，auto将无法正常获取表达式结果类型。 +template auto eval(const T & result) {return result;} + +template +auto eval(const CwiseBinaryOp & result) +{ + // The eval() function returns the result of the operation + // If this is not used auto will not work correctly + return result.eval(); +} + +template +auto eval(const Block & result) +{ + return result.eval(); +} + +} // namespace Eigen + +namespace rmoss_util +{ + +/** + * @class KalmanFilter + * + * @brief 可自动微分的扩展卡尔曼滤波器 + * + * @param XN 状态量维度 + */ +template +class KalmanFilter +{ + using VectorX = Eigen::Matrix; + using MatrixXX = Eigen::Matrix; + +public: + KalmanFilter(const VectorX & x, const MatrixXX & P) + : x_(x), P_(P) {} + + /** + * @brief 重置状态 + */ + void reset(const VectorX & x, const MatrixXX & P) + { + x_ = x; + P_ = P; + } + + /** + * 预测步 + * @brief 使用自动微分的雅可比矩阵，预测状态估计 + * @tparam 状态转移方程f(x,u)，Functor + * @tparam 控制量类型 + * @param transition_model 状态转移方程 + * @param Q 过程噪声协方差 + * @param u 控制量 + */ + template + VectorX predict( + StateTransitionModel transition_model, const MatrixXX & Q, + const Controls &... u) + { + // 绑定控制参数到状态转移方程 + auto f = std::bind(transition_model, std::placeholders::_1, u ...); + + // x_jet(i).v[i]保存状态量x_(i)的导数 + Eigen::Matrix, XN, 1> x_jet; + x_jet.setZero(); + for (int i = 0; i < XN; i++) { + x_jet(i).a = x_(i); + // 对自身求导为1 + x_jet(i).v[i] = 1.0; + } + + // 状态转移 + f(x_jet); + + // 获取雅可比矩阵同时更新状态量 + for (int i = 0; i < XN; ++i) { + x_(i) = x_jet[i].a; + F_.row(i) = x_jet[i].v.transpose(); + } + + P_ = F_ * P_ * F_.transpose() + Q; + + return x_; + } + + /** + * 更新步 + * @tparam Measurement 测量量类型 + * @tparam MeasurementModel 观测方程 h(x,v)，Functor + * @param measurementModel 观测方程 + * @param R 测量噪声协方差 + * @param z 测量值 + */ + template> + VectorX update( + MeasurementModel h, + const Eigen::Matrix, + Eigen::dof> & R, + const Measurement & z) + { + // 观测方程的雅可比矩阵 + Eigen::Matrix H; + Eigen::Matrix z_hat; + H.setZero(); + z_hat.setZero(); + + // x_jet(i).v[i]保存状态量x_(i)的导数 + Eigen::Matrix, XN, 1> x_jet; + for (int i = 0; i < XN; i++) { + x_jet(i).a = x_(i); + // 对自身求导为1 + x_jet(i).v[i] = 1.0; + } + + // 观测方程 + auto z_jet = Eigen::eval(h(x_jet)); + + for (int i = 0; i < ZN; i++) { + z_hat(i) = z_jet[i].a; + H.row(i) = z_jet[i].v.transpose(); + } + auto K = Eigen::eval(P_ * H.transpose() * (H * P_ * H.transpose() + R).inverse()); + + x_ = x_ + K * (z - z_hat); + P_ = (MatrixXX::Identity() - K * H) * P_; + + return x_; + } + +private: + // 当前状态分布的均值 + VectorX x_; + + // 当前状态分布的协方差 + MatrixXX P_; + + // 状态转移方程的雅可比矩阵 + MatrixXX F_; +}; +} // namespace rmoss_util +#endif // RMOSS_UTIL__KALMAN_FILTER_HPP_ diff --git a/rmoss_util/include/rmoss_util/mono_measure_tool.hpp b/rmoss_util/math/include/rmoss_util/mono_measure_tool.hpp similarity index 96% rename from rmoss_util/include/rmoss_util/mono_measure_tool.hpp rename to rmoss_util/math/include/rmoss_util/mono_measure_tool.hpp index 6260ce3..7abe564 100644 --- a/rmoss_util/include/rmoss_util/mono_measure_tool.hpp +++ b/rmoss_util/math/include/rmoss_util/mono_measure_tool.hpp @@ -44,7 +44,7 @@ class MonoMeasureTool */ bool solve_pnp( std::vector & points2d, std::vector & points3d, - cv::Point3f & position); + cv::Point3f & position, cv::SolvePnPMethod method = cv::SOLVEPNP_ITERATIVE); /** * @brief 逆投影，已知深度，2d->3d点求解 * diff --git a/rmoss_util/src/mono_measure_tool.cpp b/rmoss_util/math/src/mono_measure_tool.cpp similarity index 97% rename from rmoss_util/src/mono_measure_tool.cpp rename to rmoss_util/math/src/mono_measure_tool.cpp index 906a0f6..8afb26e 100644 --- a/rmoss_util/src/mono_measure_tool.cpp +++ b/rmoss_util/math/src/mono_measure_tool.cpp @@ -37,7 +37,8 @@ bool MonoMeasureTool::set_camera_info( } bool MonoMeasureTool::solve_pnp( - std::vector & points2d, std::vector & points3d, cv::Point3f & position) + std::vector & points2d, std::vector & points3d, cv::Point3f & position, + cv::SolvePnPMethod method) { if (points2d.size() != points3d.size()) { return false; // 投影点数量不匹配 @@ -45,7 +46,7 @@ bool MonoMeasureTool::solve_pnp( // cv::Mat rot = cv::Mat::eye(3, 3, CV_64FC1); cv::Mat trans = cv::Mat::zeros(3, 1, CV_64FC1); cv::Mat r; // 旋转向量 - solvePnP(points3d, points2d, camera_intrinsic_, camera_distortion_, r, trans); + solvePnP(points3d, points2d, camera_intrinsic_, camera_distortion_, r, trans, method); position = cv::Point3f(trans); return true; } diff --git a/rmoss_util/math/test/CMakeLists.txt b/rmoss_util/math/test/CMakeLists.txt new file mode 100644 index 0000000..6b93824 --- /dev/null +++ b/rmoss_util/math/test/CMakeLists.txt @@ -0,0 +1,4 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_kalman_filter test_kalman_filter.cpp) +target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math) \ No newline at end of file diff --git a/rmoss_util/math/test/test_kalman_filter.cpp b/rmoss_util/math/test/test_kalman_filter.cpp new file mode 100644 index 0000000..7d1135d --- /dev/null +++ b/rmoss_util/math/test/test_kalman_filter.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 RoboMaster-OSS +// +// 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. + +#include + +#include + +#include + +#include "rmoss_util/kalman_filter.hpp" + +// 状态量维度 +constexpr unsigned int XN = 2; + +// 定义状态转移方程 +struct StateTransitionModel +{ + StateTransitionModel(double dt) + : dt_(dt) {} + template + Eigen::Matrix operator()(const Eigen::Matrix & x) const + { + Eigen::Matrix x_new; + x_new[0] = x[0] + x[1] * dt_; + x_new[1] = x[1]; + return x_new; + } + + double dt_; +}; + +// 定义观测方程 +struct MeasurementModel +{ + template + Eigen::Matrix operator()(const Eigen::Matrix & x) const + { + Eigen::Matrix z; + z[0] = x[0]; + return z; + } +}; + +TEST(KalmanFilterTest, MultipleIterations) { + std::random_device rd; + std::mt19937 gen(rd()); + std::normal_distribution dist(0.0, 0.1); + + Eigen::Matrix x; + x << 0, 0; + Eigen::Matrix P; + P << 1, 0, 0, 1; + Eigen::Matrix Q; + Q << 0.1, 0.001, 0.001, 0.1; + + rmoss_util::KalmanFilter kf(x, P); + + for (int i = 0; i < 100; ++i) { + // 预测步骤 + kf.predict(StateTransitionModel(1.0), Q); + + // 生成假设的测量值 + Eigen::Matrix z; + z << (i + 1 + dist(gen)); + + // 更新步骤 + Eigen::Matrix R; + R << 0.001; + + x = kf.update(MeasurementModel(), R, z); + } + + // 最终状态值检查 + EXPECT_NEAR(x[0], 100.0, 1.0); + EXPECT_NEAR(x[1], 1.0, 0.1); +} diff --git a/rmoss_util/package.xml b/rmoss_util/package.xml index e19c13e..4728e5e 100644 --- a/rmoss_util/package.xml +++ b/rmoss_util/package.xml @@ -13,6 +13,8 @@ ament_index_cpp rcpputils libopencv-dev + libeigen3-dev + libceres-dev rmoss_interfaces ament_lint_auto ament_lint_common From 61f5dbec6901b7ddeb8638d84652fe40f10293cd Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 18:58:21 +0800 Subject: [PATCH 02/10] add build_depend in rmoss_util/package.xml --- rmoss_util/package.xml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rmoss_util/package.xml b/rmoss_util/package.xml index 4728e5e..0c6f717 100644 --- a/rmoss_util/package.xml +++ b/rmoss_util/package.xml @@ -9,6 +9,9 @@ zhenpeng ge Apache License 2.0 ament_cmake + + libunwind-dev + rclcpp ament_index_cpp rcpputils @@ -21,4 +24,4 @@ ament_cmake - \ No newline at end of file + From 1f4480aaa1699cf617e16d740a8e19aaf6b3556d Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:10:41 +0800 Subject: [PATCH 03/10] fix ament_target_dependencies() failed in Eigen3 --- rmoss_util/math/CMakeLists.txt | 2 ++ rmoss_util/math/test/test_kalman_filter.cpp | 2 +- rmoss_util/package.xml | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/rmoss_util/math/CMakeLists.txt b/rmoss_util/math/CMakeLists.txt index 4e8586e..d424939 100644 --- a/rmoss_util/math/CMakeLists.txt +++ b/rmoss_util/math/CMakeLists.txt @@ -1,5 +1,6 @@ # find package find_package(Eigen3 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) find_package(Ceres REQUIRED) find_package(OpenCV REQUIRED) @@ -7,6 +8,7 @@ find_package(OpenCV REQUIRED) aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR}/src MATH_SRCS) add_library(math SHARED ${MATH_SRCS}) ament_target_dependencies(math + eigen3_cmake_module Eigen3 Ceres OpenCV diff --git a/rmoss_util/math/test/test_kalman_filter.cpp b/rmoss_util/math/test/test_kalman_filter.cpp index 7d1135d..a5de92b 100644 --- a/rmoss_util/math/test/test_kalman_filter.cpp +++ b/rmoss_util/math/test/test_kalman_filter.cpp @@ -26,7 +26,7 @@ constexpr unsigned int XN = 2; // 定义状态转移方程 struct StateTransitionModel { - StateTransitionModel(double dt) + explicit StateTransitionModel(double dt) : dt_(dt) {} template Eigen::Matrix operator()(const Eigen::Matrix & x) const diff --git a/rmoss_util/package.xml b/rmoss_util/package.xml index 0c6f717..fd20641 100644 --- a/rmoss_util/package.xml +++ b/rmoss_util/package.xml @@ -9,6 +9,7 @@ zhenpeng ge Apache License 2.0 ament_cmake + eigen3_cmake_module libunwind-dev From e376f102d3a28d50e2715b99baaefe8b92f4705e Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:19:18 +0800 Subject: [PATCH 04/10] export dependencies of eigen3_cmake_module --- rmoss_util/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmoss_util/CMakeLists.txt b/rmoss_util/CMakeLists.txt index 638283a..9c17396 100644 --- a/rmoss_util/CMakeLists.txt +++ b/rmoss_util/CMakeLists.txt @@ -41,6 +41,7 @@ ament_export_dependencies(OpenCV) ament_export_dependencies(rmoss_interfaces) ament_export_dependencies(rcpputils) ament_export_dependencies(ament_index_cpp) +ament_export_dependencies(eigen3_cmake_module) ament_export_dependencies(Eigen3) ament_export_dependencies(Ceres) @@ -52,4 +53,4 @@ if(BUILD_TESTING) add_subdirectory(math/test) endif() -ament_package() \ No newline at end of file +ament_package() From 74fc552fb23a29afd8647f5fdffef7ca740cd673 Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:34:56 +0800 Subject: [PATCH 05/10] link eigen3 in rmoss_util/test/CmakeLists.txt --- rmoss_util/math/test/CMakeLists.txt | 5 ++++- rmoss_util/math/test/test_kalman_filter.cpp | 2 +- rmoss_util/package.xml | 7 ++++++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/rmoss_util/math/test/CMakeLists.txt b/rmoss_util/math/test/CMakeLists.txt index 6b93824..93cb34c 100644 --- a/rmoss_util/math/test/CMakeLists.txt +++ b/rmoss_util/math/test/CMakeLists.txt @@ -1,4 +1,7 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(Eigen3 REQUIRED) + ament_add_gtest(test_kalman_filter test_kalman_filter.cpp) -target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math) \ No newline at end of file +target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math) +target_include_directories(test_kalman_filter PUBLIC ${Eigen3_INCLUDE_DIRS}) \ No newline at end of file diff --git a/rmoss_util/math/test/test_kalman_filter.cpp b/rmoss_util/math/test/test_kalman_filter.cpp index a5de92b..89682bb 100644 --- a/rmoss_util/math/test/test_kalman_filter.cpp +++ b/rmoss_util/math/test/test_kalman_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 RoboMaster-OSS +// Copyright 2024 RoboMaster-OSS // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmoss_util/package.xml b/rmoss_util/package.xml index fd20641..868632c 100644 --- a/rmoss_util/package.xml +++ b/rmoss_util/package.xml @@ -12,16 +12,21 @@ eigen3_cmake_module libunwind-dev + eigen + rclcpp ament_index_cpp rcpputils libopencv-dev - libeigen3-dev libceres-dev rmoss_interfaces ament_lint_auto ament_lint_common + + eigen3_cmake_module + eigen + ament_cmake From 68db866a234e16362bc14c97315b1ed7ef6e3735 Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:40:53 +0800 Subject: [PATCH 06/10] add getter of KalmanFilter --- rmoss_util/math/include/rmoss_util/kalman_filter.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rmoss_util/math/include/rmoss_util/kalman_filter.hpp b/rmoss_util/math/include/rmoss_util/kalman_filter.hpp index 39a6eb6..0b0ec48 100644 --- a/rmoss_util/math/include/rmoss_util/kalman_filter.hpp +++ b/rmoss_util/math/include/rmoss_util/kalman_filter.hpp @@ -159,6 +159,10 @@ class KalmanFilter return x_; } + VectorX get_state() const {return x_;} + + MatrixXX get_covariance() const {return P_;} + private: // 当前状态分布的均值 VectorX x_; From 6f0b8a59c0d3da1305988797ababa43e45da4a13 Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:45:33 +0800 Subject: [PATCH 07/10] link Eigen3::Eigen --- rmoss_util/math/test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmoss_util/math/test/CMakeLists.txt b/rmoss_util/math/test/CMakeLists.txt index 93cb34c..a695dd0 100644 --- a/rmoss_util/math/test/CMakeLists.txt +++ b/rmoss_util/math/test/CMakeLists.txt @@ -3,5 +3,5 @@ find_package(ament_cmake_gtest REQUIRED) find_package(Eigen3 REQUIRED) ament_add_gtest(test_kalman_filter test_kalman_filter.cpp) -target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math) +target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math Eigen3::Eigen) target_include_directories(test_kalman_filter PUBLIC ${Eigen3_INCLUDE_DIRS}) \ No newline at end of file From 52a9a8603931a65d801ac5fa5c6fa1fcbca89ecd Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 19:53:37 +0800 Subject: [PATCH 08/10] update workflow --- .github/workflows/ci.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2d107d3..eff7df4 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -16,6 +16,9 @@ jobs: uses: actions/checkout@v3 with: fetch-depth: 0 + - name: Pre install + run: | + /usr/bin/bash -c "apt-get update && apt-get install -y libunwind-dev libceres-dev" - name: Build rmoss_core uses: ros-tooling/action-ros-ci@v0.2 with: From 29e716ef75909b5c48c17ee51efea8385c07b8c9 Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 21:35:57 +0800 Subject: [PATCH 09/10] rename KalmanFilter to ExtendedKalmanFilter --- rmoss_util/README.md | 8 +++++--- ...{kalman_filter.hpp => extended_kalman_filter.hpp} | 12 ++++++------ rmoss_util/math/test/CMakeLists.txt | 3 +-- rmoss_util/math/test/test_kalman_filter.cpp | 6 +++--- 4 files changed, 15 insertions(+), 14 deletions(-) rename rmoss_util/math/include/rmoss_util/{kalman_filter.hpp => extended_kalman_filter.hpp} (94%) diff --git a/rmoss_util/README.md b/rmoss_util/README.md index 4b53ee1..9c0ce26 100644 --- a/rmoss_util/README.md +++ b/rmoss_util/README.md @@ -14,7 +14,7 @@ rmoss_util是rmoss_core 中的一个公共基础包，提供一些公共基础 * `time_utils.hpp/cpp`（不建议使用，开发中） : 时间工具，用于测量运行时间。 * `mono_measure_tool.hpp/cpp`（不建议使用，开发中） : 单目测量工具类，单目算法封装（PNP解算，相似三角形反投影等） * `url_resolver.hpp/cpp` : URL 解析器，用于解析类似 camera_info_manager 的 URL，便于灵活路径管理 -* `kalman_filter.hpp/cpp`:卡尔曼滤波器，利用Ceres的自动微分功能实现雅克比矩阵的自动求解 +* `extended_kalman_filter.hpp/cpp`:扩展卡尔曼滤波器，利用Ceres的自动微分功能实现雅克比矩阵的自动求解 ## 快速使用 @@ -114,7 +114,8 @@ std::result = rmoss_util::URLResolver::getResolvedPath(url); // result = "/test ```cpp struct Predict { Predict(double dt) : dt(dt) {} - + + template Eigen::Matrix operator()( const Eigen::Matrix & x) const { @@ -128,6 +129,7 @@ struct Predict { }; struct Measure { + template Eigen::Matrix operator()( const Eigen::Matrix & x) const { @@ -138,7 +140,7 @@ struct Measure { }; // x: 初始状态, P: 初始协方差矩阵 -rmoss_util::KalmanFilter<2> ekf(x, P); +rmoss_util::ExtendedKalmanFilter<2> ekf(x, P); for (int i = 0; i < 10; i++) { // 预测步 diff --git a/rmoss_util/math/include/rmoss_util/kalman_filter.hpp b/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp similarity index 94% rename from rmoss_util/math/include/rmoss_util/kalman_filter.hpp rename to rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp index 0b0ec48..1d4b4b7 100644 --- a/rmoss_util/math/include/rmoss_util/kalman_filter.hpp +++ b/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMOSS_UTIL__KALMAN_FILTER_HPP_ -#define RMOSS_UTIL__KALMAN_FILTER_HPP_ +#ifndef RMOSS_UTIL__EXTENDED_KALMAN_FILTER_HPP_ +#define RMOSS_UTIL__EXTENDED_KALMAN_FILTER_HPP_ #include #include @@ -49,20 +49,20 @@ namespace rmoss_util { /** - * @class KalmanFilter + * @class ExtendedKalmanFilter * * @brief 可自动微分的扩展卡尔曼滤波器 * * @param XN 状态量维度 */ template -class KalmanFilter +class ExtendedKalmanFilter { using VectorX = Eigen::Matrix; using MatrixXX = Eigen::Matrix; public: - KalmanFilter(const VectorX & x, const MatrixXX & P) + ExtendedKalmanFilter(const VectorX & x, const MatrixXX & P) : x_(x), P_(P) {} /** @@ -174,4 +174,4 @@ class KalmanFilter MatrixXX F_; }; } // namespace rmoss_util -#endif // RMOSS_UTIL__KALMAN_FILTER_HPP_ +#endif // RMOSS_UTIL__EXTENDED_KALMAN_FILTER_HPP_ diff --git a/rmoss_util/math/test/CMakeLists.txt b/rmoss_util/math/test/CMakeLists.txt index a695dd0..4881e32 100644 --- a/rmoss_util/math/test/CMakeLists.txt +++ b/rmoss_util/math/test/CMakeLists.txt @@ -3,5 +3,4 @@ find_package(ament_cmake_gtest REQUIRED) find_package(Eigen3 REQUIRED) ament_add_gtest(test_kalman_filter test_kalman_filter.cpp) -target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math Eigen3::Eigen) -target_include_directories(test_kalman_filter PUBLIC ${Eigen3_INCLUDE_DIRS}) \ No newline at end of file +target_link_libraries(test_kalman_filter ${PROJECT_NAME}::math Eigen3::Eigen) \ No newline at end of file diff --git a/rmoss_util/math/test/test_kalman_filter.cpp b/rmoss_util/math/test/test_kalman_filter.cpp index 89682bb..f22c0d0 100644 --- a/rmoss_util/math/test/test_kalman_filter.cpp +++ b/rmoss_util/math/test/test_kalman_filter.cpp @@ -18,7 +18,7 @@ #include -#include "rmoss_util/kalman_filter.hpp" +#include "rmoss_util/extended_kalman_filter.hpp" // 状态量维度 constexpr unsigned int XN = 2; @@ -52,7 +52,7 @@ struct MeasurementModel } }; -TEST(KalmanFilterTest, MultipleIterations) { +TEST(ExtendedKalmanFilterTest, MultipleIterations) { std::random_device rd; std::mt19937 gen(rd()); std::normal_distribution dist(0.0, 0.1); @@ -64,7 +64,7 @@ TEST(KalmanFilterTest, MultipleIterations) { Eigen::Matrix Q; Q << 0.1, 0.001, 0.001, 0.1; - rmoss_util::KalmanFilter kf(x, P); + rmoss_util::ExtendedKalmanFilter kf(x, P); for (int i = 0; i < 100; ++i) { // 预测步骤 From 1b0d23e72f9e2a83e66666f585e6802fdd40e01c Mon Sep 17 00:00:00 2001 From: baiyeweiguang Date: Mon, 22 Jul 2024 21:51:02 +0800 Subject: [PATCH 10/10] fix a bug --- .../math/include/rmoss_util/extended_kalman_filter.hpp | 6 +++--- rmoss_util/math/test/test_kalman_filter.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp b/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp index 1d4b4b7..34bad36 100644 --- a/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp +++ b/rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp @@ -101,12 +101,12 @@ class ExtendedKalmanFilter } // 状态转移 - f(x_jet); + auto x_p_jet = f(x_jet); // 获取雅可比矩阵同时更新状态量 for (int i = 0; i < XN; ++i) { - x_(i) = x_jet[i].a; - F_.row(i) = x_jet[i].v.transpose(); + x_(i) = x_p_jet[i].a; + F_.row(i) = x_p_jet[i].v.transpose(); } P_ = F_ * P_ * F_.transpose() + Q; diff --git a/rmoss_util/math/test/test_kalman_filter.cpp b/rmoss_util/math/test/test_kalman_filter.cpp index f22c0d0..a8a8aed 100644 --- a/rmoss_util/math/test/test_kalman_filter.cpp +++ b/rmoss_util/math/test/test_kalman_filter.cpp @@ -62,11 +62,11 @@ TEST(ExtendedKalmanFilterTest, MultipleIterations) { Eigen::Matrix P; P << 1, 0, 0, 1; Eigen::Matrix Q; - Q << 0.1, 0.001, 0.001, 0.1; + Q << 0.01, 0, 0, 0.01; rmoss_util::ExtendedKalmanFilter kf(x, P); - for (int i = 0; i < 100; ++i) { + for (int i = 0; i < 20; ++i) { // 预测步骤 kf.predict(StateTransitionModel(1.0), Q); @@ -76,12 +76,12 @@ TEST(ExtendedKalmanFilterTest, MultipleIterations) { // 更新步骤 Eigen::Matrix R; - R << 0.001; + R << 0.01; x = kf.update(MeasurementModel(), R, z); } // 最终状态值检查 - EXPECT_NEAR(x[0], 100.0, 1.0); + EXPECT_NEAR(x[0], 20.0, 1.0); EXPECT_NEAR(x[1], 1.0, 0.1); }