#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);

#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;
}

#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);

#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);