yq_2026_duo/src/rm_utils/README.md

rm_utils

1. 介绍

该模块提供通用的工具类以及函数，包括：

• 扩展卡尔曼滤波器 ExtendedKalmanFilter
• PnP解算器 PnPSolver
• 弹道补偿器 TrajectoryCompensator
• FYT日志库
• URL路径解析器 URLResolver
• 心跳发布者 HeartBeatPublisher

2. 使用方法

2.0 扩展卡尔曼滤波

见armor_solver/src/armor_solver_node.cpp

2.1 PnP解算

示例：

#include "rm_utils/math/pnp_solver.hpp"

// 1. 创建PnP解算器
// 构造函数参数(cv::Mat, cv::Mat, cv::PnPMethod)：相机内参矩阵、畸变系数、解算方法（默认是IPPE）
auto pnp_solver = PnPSolver(camera_matrix, distortion_coefficients);
// 2. 为PnPSolver设置一个物体坐标系
std::vector<cv::Point3f> object_points = {
    cv::Point3f(-1.0f, 1.0f, 0.0f),
    cv::Point3f(1.0f, 1.0f, 0.0f),
    cv::Point3f(1.0f, -1.0f, 0.0f),
    cv::Point3f(-1.0f, -1.0f, 0.0f)
};
pnp_solver.setObjectPoints(object_points, "object_frame");
// 3. PnP解算
cv::Mat rvec, tvec;
pnp_solver.solvePnP(image_points, rvec, tvec, "object_frame");

// 对于打符来说，因为以前的PnP是直接返回一个表示位姿的Eigen::VectorXd，所以这里也提供了一个函数，用于将rvec和tvec转换为VectorXd
// 4. 获取位姿
Eigen::VectorXd pose = pnp_solver.getPose(rvec, tvec);

2.2 弹道补偿

示例：

#include "rm_utils/math/trajectory_compensator.hpp"

auto compensator = CompensatorFactory::createCompensator("ideal");
double pitch = 0;
Eigen::Vector3d p = Eigen::Vector3d(1.0, 0, 0);
double temp_pitch = pitch;
if (trajectory_compensator_->compensate(p, temp_pitch)) {
    pitch = temp_pitch;
}

2.3 Eigen和cv::Mat的相互转换

示例：

#include "rm_utils/math/utils.hpp"

cv::Mat mat = cv::Mat::eye(3, 3, CV_32FC1);
Eigen::Matrix3f eigen_mat = utils::cvToEigen(mat);
cv::Mat mat2 = utils::eigenToCv(eigen_mat);

2.4 FYT日志库

示例：

#include "rm_utils/logger/log.hpp"

// 1. 初始化
// 参数：日志名称、日志文件路径、日志级别
FYT_REGISTER_LOGGER("test_logger", "/tmp/test_logger.log", INFO);

// 2. 使用
FYT_INFO("test_logger", "This is a test log");
int a = 1;
FYT_WARN("test_logger", "a = {}", a);

2.5 URL Resolver

能够用很方便的方式（类camera_info_url）访问文件和或ros2包的install目录

示例:

#include "rm_utils/url_resolver.hpp"

namespace fs = std::filesystem;
fs::path model_path =
  utils::URLResolver::getResolvedPath("package://armor_detector/model/lenet.onnx");

if (fs::exists(model_path)) {
    std::cout<<model_path.string()<<std::endl;
}

fs::path file_path = utils::URLResolver::getResolvedPath("file://home/zcf/123.txt");

2.6 HearBeatPublisher

定时发布心跳数据

示例：

#include "rm_utils/heartbeat.hpp"

class TestNode : public rclcp::Node {
public:
  TestNode(const rclcpp::NodeOptions& options) : Node("test", opitions) {
    heartbeat_ = HeartBeatPublisher::create(this); // 传入Node*
  }
private:
  HeartBeatPublisher::SharedPtr heartbeat_;
};