CV::projectPoints()，cvProjectPoints2()，cvProjectPoints2Internal()的解释（点云投影）

Fournullyouth

已于 2023-12-04 14:55:35 修改

阅读量3.5k

点赞数 18

文章标签：数码相机计算机视觉人工智能 opencv

于 2023-12-04 14:15:09 首次发布

本文链接：https://blog.csdn.net/Fournullyouth/article/details/134782423

版权

将点云投影到相机平面时需要用到cv库中的CV::projectPoints()函数。

该函数调用了cvProjectPoints2()，而后者又调用cvProjectPoints2Internal()来最终实现。

下面逐函数分析。

一、CV::projectPoints()

CV::projectPoints函数的输入参数包括：

_opoints：输入的三维点坐标，大小为N×1×3或3×N的矩阵。
_rvec：旋转向量，描述相机的旋转姿态。
_tvec：平移向量，描述相机的平移姿态。
_cameraMatrix：相机内参矩阵，描述相机的内部参数。
_distCoeffs：畸变系数，描述相机的畸变特性。
_ipoints：输出的二维图像点坐标，大小为N×1×2或2×N的矩阵。
_jacobian：可选的输出参数，用于计算函数关于输入参数的雅可比矩阵。
aspectRatio：可选的参数，用于指定输出图像点的宽高比。

函数首先对输入参数进行检查和验证，确保输入参数的维度和类型满足要求。然后，函数创建了一些临时变量和矩阵，用于存储中间结果和输出结果。

接下来，函数调用了cvProjectPoints2函数，实际执行了点的投影计算。该函数接受输入参数，并根据相机的内参、畸变参数等进行点的投影计算，将结果存储在输出参数_ipoints中。如果需要计算雅可比矩阵，还会将结果存储在输出参数_jacobian中。

最后，函数执行完毕，返回投影后的二维图像点坐标。

代码：

void cv::projectPoints( InputArray _opoints,
                        InputArray _rvec,
                        InputArray _tvec,
                        InputArray _cameraMatrix,
                        InputArray _distCoeffs,
                        OutputArray _ipoints,
                        OutputArray _jacobian,
                        double aspectRatio )
{
    Mat opoints = _opoints.getMat();
    int npoints = opoints.checkVector(3), depth = opoints.depth();
    if (npoints < 0)
        opoints = opoints.t();
    npoints = opoints.checkVector(3);
    CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));

    if (opoints.cols == 3)
        opoints = opoints.reshape(3);

    CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
    CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;

    CV_Assert( _ipoints.needed() );

    _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
    Mat imagePoints = _ipoints.getMat();
    CvMat c_imagePoints = cvMat(imagePoints);
    CvMat c_objectPoints = cvMat(opoints);
    Mat cameraMatrix = _cameraMatrix.getMat();

    Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
    CvMat c_cameraMatrix = cvMat(cameraMatrix);
    CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);

    double dc0buf[5]={0};
    Mat dc0(5,1,CV_64F,dc0buf);
    Mat distCoeffs = _distCoeffs.getMat();
    if( distCoeffs.empty() )
        distCoeffs = dc0;
    CvMat c_distCoeffs = cvMat(distCoeffs);
    int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;

    Mat jacobian;
    if( _jacobian.needed() )
    {
        _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
        jacobian = _jacobian.getMat();
        pdpdrot = &(dpdrot = cvMat(jacobian.colRange(0, 3)));
        pdpdt = &(dpdt = cvMat(jacobian.colRange(3, 6)));
        pdpdf = &(dpdf = cvMat(jacobian.colRange(6, 8)));
        pdpdc = &(dpdc = cvMat(jacobian.colRange(8, 10)));
        pdpddist = &(dpddist = cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
    }

    cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
                      &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
}

分部分解释：

void cv::projectPoints( InputArray _opoints,
                        InputArray _rvec,
                        InputArray _tvec,
                        InputArray _cameraMatrix,
                        InputArray _distCoeffs,
                        OutputArray _ipoints,
                        OutputArray _jacobian,
                        double aspectRatio )

这是一个名为`projectPoints`的函数，用于将三维点投影到二维图像平面上。它接受一些输入参数和输出参数，包括输入的三维点坐标、旋转向量、平移向量、相机内参矩阵、畸变系数，以及输出的二维图像点坐标和可选的雅可比矩阵。

Mat opoints = _opoints.getMat();
int npoints = opoints.checkVector(3), depth = opoints.depth();
if (npoints < 0)
    opoints = opoints.t();
npoints = opoints.checkVector(3);
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));

这段代码将输入的三维点坐标`_opoints`转换为`Mat`类型的变量`opoints`。然后，检查`opoints`的维度和深度，确保其为3维向量。如果维度不符合要求，则对`opoints`进行转置操作。最后，检查`opoints`的维度和深度是否满足要求。

if (opoints.cols == 3)
    opoints = opoints.reshape(3);

如果`opoints`的列数为3，则将其重新调整为3行。

CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;

这段代码定义了一些`CvMat`类型的变量，用于存储雅可比矩阵的不同部分。

CV_Assert( _ipoints.needed() );

检查输出参数`_ipoints`是否被需要，确保其不为空。

其中，CV_Assert(）代表检查括号内表达式是否为真，不为真则报错。

_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
Mat imagePoints = _ipoints.getMat();
CvMat c_imagePoints = cvMat(imagePoints);
CvMat c_objectPoints = cvMat(opoints);
Mat cameraMatrix = _cameraMatrix.getMat();

创建输出参数`_ipoints`，指定其大小和类型。然后，将其转换为`Mat`类型的变量`imagePoints`。将输入参数`_opoints`和`_cameraMatrix`转换为`CvMat`类型的变量`c_objectPoints`和`c_cameraMatrix`。


Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
CvMat c_cameraMatrix = cvMat(cameraMatrix);
CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);

将输入参数`_rvec`和`_tvec`转换为`Mat`类型的变量`rvec`和`tvec`，以及相应的`CvMat`类型的变量。

double dc0buf[5]={0};
Mat dc0(5,1,CV_64F,dc0buf);
Mat distCoeffs = _distCoeffs.getMat();
if( distCoeffs.empty() )
    distCoeffs = dc0;
CvMat c_distCoeffs = cvMat(distCoeffs);
int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;

创建一个5x1的`Mat`类型的变量`dc0`，并将其转换为`CvMat`类型的变量`c_distCoeffs`。将输入参数`_distCoeffs`转换为`Mat`类型的变量`distCoeffs`，并检查其是否为空。如果为空，则将`distCoeffs`赋值为`dc0`。

Mat jacobian;
if( _jacobian.needed() )
{
    _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
    jacobian = _jacobian.getMat();
    pdpdrot = &(dpdrot = cvMat(jacobian.colRange(0, 3)));
    pdpdt = &(dpdt = cvMat(jacobian.colRange(3, 6)));
    pdpdf = &(dpdf = cvMat(jacobian.colRange(6, 8)));
    pdpdc = &(dpdc = cvMat(jacobian.colRange(8, 10)));
    pdpddist = &(dpddist = cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
}

如果输出参数`_jacobian`被需要，则创建一个大小为npoints*2x(3+3+2+2+ndistCoeffs)的`Mat`类型的变量`jacobian`。然后，将其分割为不同部分，并创建相应的`CvMat`类型的变量。

cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
                  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );

调用`cvProjectPoints2`函数进行点的投影计算。将输入参数和输出参数传递给该函数，以及之前创建的雅可比矩阵的部分。

整个函数执行完毕后，投影后的二维图像点坐标存储在输出参数`_ipoints`中，如果需要，雅可比矩阵存储在输出参数`_jacobian`中。

二、cvProjectPoints2()

cvProjectPoints2()函数的代码如下

CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
                  const CvMat* r_vec,
                  const CvMat* t_vec,
                  const CvMat* A,
                  const CvMat* distCoeffs,
                  CvMat* imagePoints, CvMat* dpdr,
                  CvMat* dpdt, CvMat* dpdf,
                  CvMat* dpdc, CvMat* dpdk,
                  double aspectRatio )
{
    cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
                              dpdf, dpdc, dpdk, NULL, aspectRatio );
}

可以看到也是调用了cvProjectPoints2Internal()函数来实现功能，所以核心投影功能在cvProjectPoints2Internal()中。

三、cvProjectPoints2Internal()

cvProjectPoints2Internal()拥有很多判断格式的步骤，这里直接看最核心的一段实现投影功能的代码。

for (int i = 0; i < count; i++)
{
    // 从数组 M 中获取第 i 个物体点的三维坐标 (X, Y, Z)
    double X = M[i].x, Y = M[i].y, Z = M[i].z;

    // 根据旋转矩阵 R、平移向量 t 和物体点的三维坐标计算投影后的二维坐标 (x, y)
    double x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
    double y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
    double z = R[6] * X + R[7] * Y + R[8] * Z + t[2];

    // 定义一些中间变量用于计算畸变
    double r2, r4, r6, a1, a2, a3, cdist, icdist2;
    double xd, yd, xd0, yd0, invProj;
    Vec3d vecTilt;
    Vec3d dVecTilt;
    Matx22d dMatTilt;
    Vec2d dXdYd;

    // 将 z 的值保存到 z0 中，并将 z 归一化为投影平面的坐标
    double z0 = z;
    z = z ? 1. / z : 1;
    x *= z;
    y *= z;

    // 计算平方和 r2、r4 和 r6
    r2 = x * x + y * y;
    r4 = r2 * r2;
    r6 = r4 * r2;

    // 计算 a1、a2 和 a3
    a1 = 2 * x * y;
    a2 = r2 + 2 * x * x;
    a3 = r2 + 2 * y * y;

    // 计算径向畸变系数 cdist 和其倒数 icdist2
    cdist = 1 + k[0] * r2 + k[1] * r4 + k[4] * r6;
    icdist2 = 1. / (1 + k[5] * r2 + k[6] * r4 + k[7] * r6);

    // 计算畸变后的 x 和 y 坐标 xd0、yd0
    xd0 = x * cdist * icdist2 + k[2] * a1 + k[3] * a2 + k[8] * r2 + k[9] * r4;
    yd0 = y * cdist * icdist2 + k[2] * a3 + k[3] * a1 + k[10] * r2 + k[11] * r4;

    // 将畸变后的坐标投影到倾斜平面上
    vecTilt = matTilt * Vec3d(xd0, yd0, 1);

    // 计算倾斜平面上的投影坐标的倒数
    invProj = vecTilt(2) ? 1. / vecTilt(2) : 1;

    // 根据相机内参和畸变后的坐标计算最终的像素坐标
    xd = invProj * vecTilt(0);
    yd = invProj * vecTilt(1);

    m[i].x = xd * fx + cx;
    m[i].y = yd * fy + cy;
}