Add KalmanFilter for rmoss_util

.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/[email protected]
         with:

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,19 @@ 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_cmake_module)
+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()
+ament_package()

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，便于灵活路径管理
+* `extended_kalman_filter.hpp/cpp`:扩展卡尔曼滤波器，利用Ceres的自动微分功能实现雅克比矩阵的自动求解
 
 ## 快速使用
 
@@ -104,4 +105,48 @@ std::result = rmoss_util::URLResolver::getResolvedPath(url);  // result = "<rmos
 
 std::string url = "file:///test_dir/test_file";
 std::result = rmoss_util::URLResolver::getResolvedPath(url);  // result = "/test_dir/test_file"
-```
+```
+
+### 卡尔曼滤波器
+
+下面是一个1维匀速运动的卡尔曼滤波示例。
+
+```cpp
+struct Predict {
+  Predict(double dt) : dt(dt) {}
+
+  template<typename T>  
+  Eigen::Matrix<T, 2, 1> operator()(
+    const Eigen::Matrix<T, 2, 1> & x) const
+  {
+    Eigen::Matrix<T, 2, 1> x_new;
+    x_new[0] = x[0] + x[1] * dt;
+    x_new[1] = x[1];
+    return x_new;
+  }
+
+  double dt;  
+};
+
+struct Measure {
+  template<typename T>
+  Eigen::Matrix<T, 1, 1> operator()(
+    const Eigen::Matrix<T, 1, 1> & x) const
+  {
+    Eigen::Matrix<T, 1, 1> z;
+    z[0] = x[0];
+    return z;
+  }
+};
+
+// x: 初始状态, P: 初始协方差矩阵
+rmoss_util::ExtendedKalmanFilter<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: 观测量  
+}
+```
+

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)

rmoss_util/test/CMakeLists.txt → 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})
+target_link_libraries(test_url_resolve ${PROJECT_NAME}::common)

rmoss_util/math/CMakeLists.txt

@@ -0,0 +1,19 @@
+# find package
+find_package(Eigen3 REQUIRED)
+find_package(eigen3_cmake_module 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_cmake_module
+  Eigen3
+  Ceres
+  OpenCV
+)
+target_include_directories(math PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)

rmoss_util/math/include/rmoss_util/extended_kalman_filter.hpp

@@ -0,0 +1,177 @@
+// 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__EXTENDED_KALMAN_FILTER_HPP_
+#define RMOSS_UTIL__EXTENDED_KALMAN_FILTER_HPP_
+
+#include <Eigen/Dense>
+#include <ceres/ceres.h>
+
+// 一些Eigen的工具函数，参考https://github.com/TomLKoller/ADEKF
+namespace Eigen
+{
+// 获取一个Eigen矩阵的行数
+template<typename Derived>
+static constexpr int dof =
+  internal::traits<typename Derived::PlainObject>::RowsAtCompileTime;
+
+// 用于绕过Eigen表达式模板，获取表达式结果。如果不使用eval()，auto将无法正常获取表达式结果类型。
+template<typename T> auto eval(const T & result) {return result;}
+
+template<typename BinaryOp, typename LhsType, typename RhsType>
+auto eval(const CwiseBinaryOp<BinaryOp, LhsType, RhsType> & result)
+{
+  // The eval() function returns the result of the operation
+  // If this is not used auto will not work correctly
+  return result.eval();
+}
+
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+auto eval(const Block<XprType, BlockRows, BlockCols, InnerPanel> & result)
+{
+  return result.eval();
+}
+
+}  // namespace Eigen
+
+namespace rmoss_util
+{
+
+/**
+ * @class ExtendedKalmanFilter
+ *
+ * @brief 可自动微分的扩展卡尔曼滤波器
+ *
+ * @param XN 状态量维度
+ */
+template<int XN>
+class ExtendedKalmanFilter
+{
+  using VectorX = Eigen::Matrix<double, XN, 1>;
+  using MatrixXX = Eigen::Matrix<double, XN, XN>;
+
+public:
+  ExtendedKalmanFilter(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<typename StateTransitionModel, typename ... Controls>
+  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<ceres::Jet<double, XN>, 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;
+    }
+
+    // 状态转移
+    auto x_p_jet = f(x_jet);
+
+    // 获取雅可比矩阵同时更新状态量
+    for (int i = 0; i < XN; ++i) {
+      x_(i) = x_p_jet[i].a;
+      F_.row(i) = x_p_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<typename Measurement, typename MeasurementModel,
+    int ZN = Eigen::dof<Measurement>>
+  VectorX update(
+    MeasurementModel h,
+    const Eigen::Matrix<double, Eigen::dof<Measurement>,
+    Eigen::dof<Measurement>> & R,
+    const Measurement & z)
+  {
+    // 观测方程的雅可比矩阵
+    Eigen::Matrix<double, ZN, XN> H;
+    Eigen::Matrix<double, ZN, 1> z_hat;
+    H.setZero();
+    z_hat.setZero();
+
+    // x_jet(i).v[i]保存状态量x_(i)的导数
+    Eigen::Matrix<ceres::Jet<double, XN>, 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_;
+  }
+
+  VectorX get_state() const {return x_;}
+
+  MatrixXX get_covariance() const {return P_;}
+
+private:
+  // 当前状态分布的均值
+  VectorX x_;
+
+  // 当前状态分布的协方差
+  MatrixXX P_;
+
+  // 状态转移方程的雅可比矩阵
+  MatrixXX F_;
+};
+}  // namespace rmoss_util
+#endif  // RMOSS_UTIL__EXTENDED_KALMAN_FILTER_HPP_

rmoss_util/include/rmoss_util/mono_measure_tool.hpp → rmoss_util/math/include/rmoss_util/mono_measure_tool.hpp

@@ -44,7 +44,7 @@ class MonoMeasureTool
    */
   bool solve_pnp(
     std::vector<cv::Point2f> & points2d, std::vector<cv::Point3f> & points3d,
-    cv::Point3f & position);
+    cv::Point3f & position, cv::SolvePnPMethod method = cv::SOLVEPNP_ITERATIVE);
   /**
    * @brief 逆投影，已知深度，2d->3d点求解
    *

rmoss_util/src/mono_measure_tool.cpp → rmoss_util/math/src/mono_measure_tool.cpp

@@ -37,15 +37,16 @@ bool MonoMeasureTool::set_camera_info(
 }
 
 bool MonoMeasureTool::solve_pnp(
-  std::vector<cv::Point2f> & points2d, std::vector<cv::Point3f> & points3d, cv::Point3f & position)
+  std::vector<cv::Point2f> & points2d, std::vector<cv::Point3f> & points3d, cv::Point3f & position,
+  cv::SolvePnPMethod method)
 {
   if (points2d.size() != points3d.size()) {
     return false;  // 投影点数量不匹配
   }
   // 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;
 }