2D毫米波雷达和3D激光雷达外参标定-单应性矩阵求解

视觉牛马

于 2024-11-20 09:03:06 发布

阅读量211

点赞数 3

文章标签： 3d 矩阵算法计算机视觉

本文链接：https://blog.youkuaiyun.com/qq_33912182/article/details/143901170

版权

以大陆408前向毫米波雷达为例，输出的障碍物为（x,y）坐标值，激光雷达输出的点云坐标为(x,y,z).可以理解为在xoy平面上，求解毫米波成像平面到激光雷达成像平面之间的透视变换，即单应性矩阵的求解，至少需要4对匹配点对。通过角反射器，分别确定在毫米波雷达和激光雷达匹配点的坐标值（x,y）。通过最小2乘法，求解单应性矩阵的8个参数。

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// 定义点的结构体
struct Point2D 
{
    double x, y;
};
// 单应性矩阵求解-2D透视变换 8个未知参数  平移加旋转加透视变换 至少4对匹配点 8个线性方程
void computeExtrinsicParameters_H(const vector<Point2D>& radarPoints, const vector<Point2D>& lidarPoints, Matrix3d& H) 
{
    int n = radarPoints.size();
    if (n != lidarPoints.size() || n < 4) 
    {
        cerr << "Error: Number of points must be at least 4 and consistent." << endl;
        return;
    }

    // 构建矩阵 A 和向量 B
    MatrixXd A(2 * n, 8);
    VectorXd B(2 * n);

    for (int i = 0; i < n; ++i)
    {