[slam]图像透视变换

一. 概述

此文档描述前视摄像头的图像透视变换到俯视图的原理和代码。

前视图和投影出来的俯视图如下：

激光点云投影到俯视图如下：

二. 相机内外参

1. 根据输入内外参获得摄像头到车系的转换矩阵：

Eigen::Quaterniond q2(-0.49558023262776935,
                            0.5038174805230502,
                            -0.5028083945881262,
                            0.4977468208635019);
Eigen::Matrix3d r = q2.toRotationMatrix();
Eigen::Vector3d t(1.965538,
                        -0.052,
                        1.447853);
Eigen::Matrix<double,4,4> RT2;
RT2.setIdentity();
RT2.block<3,3>(0,0) = r;
RT2.block<3,1>(0,3) = t;

注意q的输入是wxyz的顺序，输入的是摄像头在车系下的位姿，得到摄像头到车系的转换矩阵，即为RTcar2lidar。

2.获得车系到摄像头的转换矩阵：

RTcar2lidar的逆矩阵即为车系到摄像头的转换矩阵

Eigen::Matrix<double,4,4> RT2_ = RT2.inverse();

3.获得内参矩阵和畸变参数

cv::Mat K2=(cv::Mat_<double>(3,3)<<7369.746246000001,  0.000000, 1934.12,
                 0.000000,  7367.73195,  1076.48,
                  0.000000, 0.000000, 1.000000);
cv::Mat D2 =(cv::Mat_<double>(1,5)<<-0.157905, -1.57416, -0.000165876, 0.000450386,  7.47766);

内参矩阵表示缩放关系和原点的平移

畸变参数中，注意输入的顺序，fisheye的输入顺序为："k1，k2，k3，k4"，wideview的输入顺序为："k1，k2，p1，p2，k3"

三. 车系点透视变换到前视

可以随便取车系中的四个点，但是这四个点投影要能落在前视图像中，同时任意三个点不共线，保证是一个四边形的点

std::vector<cv::Point3d> world_points_vec2;
world_points_vec2.push_back(cv::Point3d(20,0,0));
world_points_vec2.push_back(cv::Point3d(40,0,0));
world_points_vec2.push_back(cv::Point3d(40,1,0));
world_points_vec2.push_back(cv::Point3d(50,5,0));

根据输入的四个点获得图像中的像素img_points_vec2，并组装成Point2f结构体的cameraPointF

std::vector<cv::Vec2d> img_points_vec2;
WorldPoints2Image(world_points_vec2, img_points_vec2, img2, RT_mat, K2, D2, distort_model);

其中WorldPoints2Image函数实现点到图像的映射，并加上图像畸变，具体函数如下：

bool WorldPoints2Image(std::vector<cv::Point3d> cloud_ptr,std::vector<cv::Vec2d>& projectPoints, cv::Mat& pic,cv::Mat RT,cv::Mat K,cv::Mat D,std::string distortModel) {
    if (cloud_ptr.size() == 0 || pic.empty() || RT.size() != cv::Size(4, 4) || K.size() != cv::Size(3, 3))
        return false;
    if (pic.depth() != CV_8U)
        return false;
    int channels = pic.channels();
    if (channels != 1 && channels != 3)
        return false;
    int height = pic.rows;
    int width = pic.cols;

    //std::vector<cv::Vec2d> projectPoints;

    if (distortModel == "K_B") {
        if (D.rows == 1 && D.cols == 4)
            D = D.t();
        if (D.rows != 4 || D.cols != 1)
            return false;
        ProjectWorldPoints_KB(cloud_ptr, projectPoints, RT, K, D, height, width);
    } else if (distortModel == "pinhole") {
        if (D.rows == 1)
            D = D.t();
        if (D.cols != 1)
            return false;
        if (D.rows != 4 && D.rows != 5 && D.rows != 8 && D.rows != 12 && D.rows != 14)
            return false;
        ProjectWorldPoints_pinhole(cloud_ptr, projectPoints, RT, K, D, height, width);

    } else
        return false;

    if (channels == 3) {
        for (size_t i = 0; i < projectPoints.size(); ++i) {
            int u = static_cast<int>(projectPoints[i](0));
            int v = static_cast<int>(projectPoints[i](1));
            cv::circle(pic,cv::Point(u,v),5,cv::Scalar(0,0,255));

        }
    } else {
        for (size_t i = 0; i < projectPoints.size(); ++i) {
            int u = static_cast<int>(projectPoints[i](0));
            int v = static_cast<int>(projectPoints[i](1));
            cv::circle(pic,cv::Point(u,v),5,cv::Scalar(255));

        }
    }
    return true;
}

其中主要的是当是wideview摄像头时执行函数

void ProjectWorldPoints_KB(std::vector<cv::Point3d> cloud_ptr, std::vector<cv::Vec2d>& imgPoints,cv::Mat RT,cv::Mat K,cv::Mat D,int imgH,int imgW){
    cv::Matx33f camMat(K);
    cv::Matx41f d(D);
    cv::Matx44f rt(RT);
    imgPoints.clear();
    for(size_t i=0;i<cloud_ptr.size();++i){
        cv::Matx41f lidarP(cloud_ptr[i].x,cloud_ptr[i].y,cloud_ptr[i].z,1);
        cv::Matx41f cameraP = rt*lidarP;
        float r_u = sqrt(cameraP(0) * cameraP(0) + cameraP(1) * cameraP(1));
        float R = sqrt(cameraP(0) * cameraP(0) + cameraP(1) * cameraP(1) + cameraP(2) * cameraP(2));
        float theta = acos(cameraP(2) / R);

        float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta4 * theta4;
        float r_d = theta * (1 + d(0) * theta2 + d(1) * theta4 + d(2) * theta6 + d(3) * theta8);

        cv::Matx31f xyz(r_d * cameraP(0) / r_u, r_d * cameraP(1) / r_u, 1.0);
        cv::Matx31f UV = camMat * xyz;
        std::cout<<"uv:"<<UV(0)<<","<<UV(1)<<std::endl;

        if(UV(0)<0||UV(1)<0||UV(0)>imgW||UV(1)>imgH)
            continue;
        imgPoints.push_back(cv::Vec2d(UV(0),UV(1)));
    }
}

当是fisheye摄像头时，执行：

void ProjectWorldPoints_pinhole(std::vector<cv::Point3d> cloud_ptr, std::vector<cv::Vec2d>& imgPoints,cv::Mat RT,cv::Mat K,cv::Mat D,int imgH,int imgW){
    float d[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    cv::Matx33f camMat(K);
    cv::Matx44f rt(RT);
    cv::Mat DistMat=cv::Mat(D.rows, D.cols, CV_32F, d);
    D.convertTo(DistMat,CV_32F,1);
    cv::Matx33f R_tao1=cv::Matx33f::eye();
    cv::Matx33f R_tao2=cv::Matx33f::eye();
    if(d[12]!=0||d[13]!=0){
        R_tao2(0,0)=cos(d[13]);
        R_tao2(0,1)=sin(d[13])*sin(d[12]);
        R_tao2(0,2)=-sin(d[13])*cos(d[12]);
        R_tao2(1,1)=cos(d[12]);
        R_tao2(1,2)=sin(d[12]);
        R_tao2(2,0)=sin(d[13]);
        R_tao2(2,1)=-cos(d[13])*sin(d[12]);
        R_tao2(2,2)=cos(d[13])*cos(d[12]);
        R_tao1(0,0)=R_tao2(2,2);
        R_tao1(1,1)=R_tao2(2,2);
        R_tao1(0,2)=-R_tao2(0,2);
        R_tao1(1,2)=-R_tao2(1,2);
    }
    size_t tmp =cloud_ptr.size();
    for(size_t i=0;i<cloud_ptr.size();++i){
        cv::Matx41f lidarP(cloud_ptr[i].x,cloud_ptr[i].y,cloud_ptr[i].z,1);
        cv::Matx41f cameraP=rt*lidarP;
        cameraP=cameraP*(1./cameraP(2));
        float x2=cameraP(0)*cameraP(0);
        float y2=cameraP(1)*cameraP(1);
        float r2=x2+y2;
        float r4=r2*r2;
        float r6=r4*r2;
        float ratio=(1+d[0]*r2+d[1]*r4+d[4]*r6)/(1+d[5]*r2+d[6]*r4+d[7]*r6);
        cv::Matx31f distP=cv::Matx31f::ones();
        distP(0)=cameraP(0)*ratio + 2*d[2]*cameraP(0)*cameraP(1) + d[3]*(r2+2*x2) + d[8]*r2 + d[9]*r4;
        distP(1)=cameraP(1)*ratio + d[2]*(r2+2*y2) + 2*d[3]*cameraP(0)*cameraP(1) + d[10]*r2 + d[11]*r4;
        distP=R_tao1*R_tao2*distP;
        cv::Matx31f UV = camMat * distP;
        std::cout<<"uv:"<<UV(0)<<","<<UV(1)<<std::endl;
        if(UV(0)<0||UV(1)<0||UV(0)>imgW||UV(1)>imgH)
            continue;
        imgPoints.push_back(cv::Vec2d(UV(0),UV(1)));
    }
}

即获得图像中的四个加了畸变参数后的uv像素点

四. 车系点透视变换到俯视

假设俯视的摄像头的高度为5m，可根据实际出来的图像调整，车系的坐标为前左上，俯视的摄像头的坐标系是右后下，所以可以写出外参变换：

Eigen::Vector3d pcn1={0,-20*0.2,1};
Eigen::Vector3d pcn2={0,-40*0.2,1};
Eigen::Vector3d pcn3={-1*0.2,-40*0.2,1};
Eigen::Vector3d pcn4={-5*0.2,-50*0.2,1};

此处也可以用变换矩阵实现，乘0.2是因为高度是5，归一化相当于xy*0.2

内参目前是自己定义的，此处有些不合理处

mat_eigen << 200,0,2000,
                0,200,2000,
                0,0,1;

根据此内参得到俯视图中的uv像素并组装成Point2f结构体的cameraPointbev1：

Eigen::Vector3d puv1 = mat_eigen * pcn1;
Eigen::Vector3d puv2 = mat_eigen * pcn2;
Eigen::Vector3d puv3 = mat_eigen * pcn3;
Eigen::Vector3d puv4 = mat_eigen * pcn4;

五. warpPerspective透视变换

根据前视图和俯视图四个点的uv像素的对应关系，获得透视变换矩阵：

cv::Mat transmatrixf = getPerspectiveTransform(cameraPointF, cameraPointbev1);

进行扭曲投影

warpPerspective(img2, Top_F, transmatrixf, cv::Size(img2.size()));

前视图和投影出来的俯视图如下所示：

六. 点云数据投影到俯视图

点云数据其实就是激光雷达系下的点坐标，先把激光雷达系下的点转换到车系下，再根据俯视相机的内外参投影到俯视图即可：

激光系到车系转换矩阵：

Eigen::Quaterniond qlidar(0.9999522261453212,
                        -0.0016523073130300901,
                        0.007759150754488417,
                        -0.005710594288589378);
Eigen::Matrix3d rlidar = qlidar.toRotationMatrix();
Eigen::Vector3d tlidar(1.7964987754821777,
                        -0.0055224779061973095,
                        1.5764740705490112);
Eigen::Matrix<double,4,4> RTlidar;
RTlidar.setIdentity();
RTlidar.block<3,3>(0,0) = rlidar;
RTlidar.block<3,1>(0,3) = tlidar;

转换投影点云坐标并显示：

Eigen::Vector4d pcar = RTlidar * plidar;
Eigen::Vector3d pcn={-pcar[1]*0.2,-pcar[0]*0.2,1};
Eigen::Vector3d puv = mat_eigen * pcn;
// 设置圆圈的半径和颜色
int radius = 1;
cv::Scalar color(0, 0, 255); // BGR颜色，这里是红色
// 在UV位置绘制红色圆圈
cv::circle(Top_F, cv::Point(puv[0], puv[1]), radius, color, -1); // -1 表示填充圆

即可获得激光点云投影到俯视图中的图像，结果如下：