[slam]相机模型和图像变换

六树蚊

已于 2024-02-01 17:40:49 修改

阅读量1.4k

点赞数 29

文章标签：数码相机

于 2024-01-22 17:36:50 首次发布

本文链接：https://blog.csdn.net/weixin_55533898/article/details/135752815

版权

一. 单目相机

1.1 相机内外参

1.1.1 相机外参

Vehicle系转换到摄像头camera系的位姿矩阵Tcv，为摄像头的外参数

1.1.2 相机内参

1. 针孔模型

摄像头系下物体P坐标[x,y,z]，成像平面上P'坐标[x′,y′,z′]，焦距为f

相似三角形可得：

$\frac{z}{f} = \frac{x}{x'} = \frac{y}{y'}$

得：

$x' = f*\frac{x}{z}$ $y' = f*\frac{y}{z}$

设像素坐标在u轴上缩放a倍，在v轴上缩放b倍，原点平移[cx,cy]，所以P'与uv之间关系：

$u = a*x'+c_x=f_x*\frac{x}{z}+c_x$

$v = b*y'+c_y=f_y*\frac{y}{z}+c_y$

所以:

K即为摄像头的内参

1.2 相机畸变

除畸变：uv经过内参先转换为归一化坐标，再计算去除畸变的归一化坐标，再经过内参计算新的uv像素

1.2.1 畸变类型

1.2.2 畸变原理

归一化平面上的点的坐标为[x,y]，也可以表示为[r,θ]，径向畸变即长度方向r发生了变化，切向畸变即水平夹角θ发生了变化，畸变后的坐标[xdistorted,ydistorted]为：

$x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)$

$y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy$

1.2.3 去畸变代码

/ 1.内参和畸变参数计算distor_x, distor_y
K = (cv::Mat_<double>(3,3) << fx,0,cx,
                            0,fy,cy,
                            0,0,1);
D = (cv::Mat_<double>(1,4) << k1,k2,p1,p2);   // 这边读取四个畸变参数，应该是省略了6次以后的高次项
cv::initUndistortRectifyMap(K, D, cv::Mat_<double>::eye(3,3), K, cv::Size(3840,2160),
                         CV_32FC1, distor_x, distor_y);
// 2.图像去畸变
cv::remap(img_src, image_dst, distor_x, distor_y, cv::INTER_LINEAR);

1.3 鱼眼相机模型

1.3.1 相机组成

1.3.2 成像模型

在研究鱼眼相机成像时，可以将上面的镜头组简化为一个球面

对于鱼眼相机，入射光线PO1经过镜头后会发生折射，因此P'的像点为p点，极坐标表示为(r,φ)。
为了将尽可能大的场景投影到有限的图像平面内，鱼眼相机会按照一定的投影函数来设计。

1. 透视映射(Perspective)

$r = f * tan(\vartheta )$

光心到成像面(相机的CCD)的距离为焦距f，空间点与光轴的夹角为θ，当ff固定，θθ增大到接近90度时，此时成像的入射光线与CCD平行，r为无穷大。

2. 等距映射(Equidistant)

r = f ∗ θ

成像点的位置R与入射角成正比，比例系数为f。与透射映射相比，解决了tan(90)为无穷大的问题。2D图像上θ对应的等高线为半径等比例变化的同心圆，最大的FOV可以达到360度，这是最简单的一种鱼眼模型。

3. 等立体角投影(Equisolid angle)

r = 2f ∗ sinθ/2

4. 正交投影(Orthographic)

r = 2f ∗ sinθ

与透射映射不同，不会产生近大远小的透视效果。2D图像上θ对应的等高线从0到90度越来越密。最大的FOV是180度。

5. 球极投影(Stereographic)

r = 2f ∗ tanθ/2

是一种将圆球面投影至平面的映射，在几何学里也称共型映射conformal mapping，是一种保角映射。
实际相机镜头的加工无法严格按照上面介绍的映射模型精确实现，一般用θ的多项式来近似r函数

$r = f\theta(1 + k_1\theta+ k_2{\theta}^2 + k_3{\theta}^3 + k_4{\theta}^4)$

所以对于鱼眼相机，需要标定的内参为焦距f，图像中光心位置Co和多项式映射函数的系数ki(一般的文档中称为径向畸变参数),有的还会引入切向畸变参数pi.引入过多的参数可能会导致优化求解过程陷入局部最小值，所以OpenCV文档中默认畸变参数(distortion coefficients) 为k1,k2,k3,k4。
在《A generic camera model and calibration method kannala brandt》中多项式是这样的:

$r(\theta) = k_1\theta+ k_2{\theta}^3 + k_3{\theta}^5 + k_4{\theta}^7$

1.3.3 3D to uv成像过程

1. give P(X,Y,Z)

2. 归一化到球平面

$\hat{P} = \frac{P}{\lVert P\rVert}=(\hat{P_x},\hat{P_y},\hat{P_z})$

3. 求入射角θ和φ

$R = \sqrt{X^2+ Y^2}$

$\tan{\theta} = \frac{R}{Z}$

$\theta = \arctan{\frac{R}{Z}}$

$\tan{\varphi } = \frac{Y}{X}$

4. 基于畸变模型得到r(θ)

$r(\theta) = k_1\theta+ k_2{\theta}^3 + k_3{\theta}^5 + k_4{\theta}^7$

5. O2像平面上坐标

$p_d = \begin{bmatrix} x \\ y \end{bmatrix} = \frac{r}{R} \begin{bmatrix} X \\ Y \end{bmatrix}$

$p_d = \begin{bmatrix} x \\ y \end{bmatrix} = r \begin{bmatrix} \cos{\varphi } \\ \sin{\varphi }\end{bmatrix}$

6. 图像像素坐标(u,v)

$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x&0&c_x \\ 0&f_y&c_y \\ 0&0&1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$

1.3.4 成像过程代码

Eigen::Vector2f project(const Eigen::Vector3f &v3D) // 摄像头系下的点坐标
    {
        const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
        const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
        const float psi = atan2f(v3D[1], v3D[0]);

        const float theta2 = theta * theta;
        const float theta3 = theta * theta2;
        const float theta5 = theta3 * theta2;
        const float theta7 = theta5 * theta2;
        const float theta9 = theta7 * theta2;
        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                         + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        Eigen::Vector2f res;
        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

        return res;
    }

1.4 变换

1.4.1 欧式变换

移动+旋转的变换

$T = \begin{bmatrix}R&t\\0&1\end{bmatrix}$

1.4.2 相似变换

比欧式变换多一个自由度，允许缩放

$T = \begin{bmatrix}sR&t\\0&1\end{bmatrix}$

1.4.3 仿射变换

仿射变换是平面变换，是二维坐标(x,y)的转换

仿射变换的方程组有6个未知数，所以要求解就需要找到3组映射点，三个点刚好确定一个平面

1.4.4 透视变换

透视变换是空间变换，是三维坐标(x,y,z)的变换

视变换的方程组有8个未知数，所以要求解就需要找到4组映射点，四个点就刚好确定了一个三维空间

计算透视变换的H矩阵，可以用opencv中的getPerspectiveTransform函数

1.4.4.1 world2image

Eigen::Vector3d AVM::Word2Image(const Eigen::Vector3d& p_car, 
                            const Eigen::Matrix3d& K, Eigen::Matrix4d T_top_b)
    {
        Eigen::Vector3d puv;   //像素平面上坐标

        Eigen::Vector4d p_car_;
        p_car_ << p_car(0), p_car(1), p_car(2), 1;
        std::cout << "车系下的坐标为：\n" << p_car_ << std::endl;
        // std::cout << "T_top_b:\n" << T_top_b << std::endl;
        
        Eigen::Vector4d Pc = T_top_b * p_car_;  // car坐标系下的点转换为camera坐标系下的点
        Eigen::Vector3d Pc_;
        Pc_ << Pc(0), Pc(1), Pc(2);
        std::cout << "摄像头坐标系下的坐标: \n" << Pc << std::endl;

        Eigen::Vector3d pcn;   //归一化
        pcn = Pc_ / Pc_(2);
        std::cout << "归一化平面上的坐标: \n" << pcn << std::endl;

        puv = K * pcn;
        std::cout << "新的像素坐标: \n" << puv << std::endl;

        return puv;
    }

1.4.4.2 image2world

原理：

代码:

Eigen::Vector3d AVM::Image2Word(const Eigen::Vector3d& p_uv, 
                            const Eigen::Matrix3d& K, Eigen::Matrix4d T_b_cf)
    {
        // std::cout << "原始像素坐标：\n" << p_uv << std::endl;
        Eigen::Vector3d Pcar;

        Eigen::Vector3d Pcn_xy = K.inverse() * p_uv;  //理论摄像头（归一化平面）坐标系下点的坐标
        // std::cout << "摄像头坐标系下归一化平面上点的坐标：\n" << Pcn_xy<< std::endl;

        Eigen::Vector4d Pcn_xy_;
        Pcn_xy_ << Pcn_xy(0), Pcn_xy(1), Pcn_xy(2), 1;
        
        Eigen::Vector4d Pcar_xy_ = T_b_cf * Pcn_xy_;

        Eigen::Vector3d Pcar_xy;
        Pcar_xy << Pcar_xy_(0), Pcar_xy_(1), Pcar_xy_(2);
        // std::cout << "整车坐标系下点的坐标：\n" << Pcar_xy<< std::endl;

        Eigen::Vector3d pp;    // 平面上一点
        pp << 0,0,0;
        Eigen::Vector3d pn;    // 平面上的法向量
        pn << 0,0,1;

        Eigen::Vector3d Tc;
        Tc = T_b_cf.block(0,3,3,1);

        // std::cout << "摄像头的位置Tc:\n" << Tc << std::endl;

        AVM::CalculateLineAndPlane(Tc, Pcar_xy, pp, pn, Pcar);   //计算出交点的坐标（坐标系为car坐标系）
        // std::cout << "交点的坐标：\n" << Pcar.transpose() << std::endl;

        return Pcar;
    }

1.4.4.3 image2image

前后等摄像头图像转换成鸟瞰图

1. 计算出单点的转换关系

Eigen::Vector3d AVM::UV2UV(const Eigen::Vector3d& puv, const Eigen::Matrix3d& K, 
                const Eigen::Matrix4d& T_b_cf, const Eigen::Matrix3d& K_top, 
                const Eigen::Matrix4d& T_top_b)
    {
        std::cout << "------原始的像素坐标: \n" << puv << std::endl;
        Eigen::Vector3d pcar;
        pcar = AVM::Image2Word(puv, K, T_b_cf);   

        Eigen::Vector3d puv_top;
        puv_top = AVM::Word2Image(pcar, K_top, T_top_b);
        std::cout << "------新的像素坐标: \n" << puv_top << std::endl;
        return puv_top;
    }

2. 计算H矩阵

计算出四个点的转换关系后，用opencv库计算H矩阵：

cv::Mat transmatrixf = getPerspectiveTransform(srcPointF, dstPointF);//得到透视变换的矩阵

3. 通过H矩阵对图像进行转换

warpPerspective(src, Top_F, transmatrixf, Size(3840, 2160));

1.4.4.4 透视变换效果

泊车仿真图像转换与拼接：

高速车道线转换：

二. 双目相机

单目相机无法计算深度信息

2.1 计算深度原理

相似三角形：

$\frac{z-f}{z} = \frac{b-u_l+u_r}{b}$

所以深度为：

$z = \frac{fb}{u_l-u_r}$

2.2 代码实现

int main(int argc, char **argv) 
{
    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(       
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // 神奇的参数
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);                  // 神奇的sgbm算法
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            // 计算归一化平面上的点
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            // 计算深度
            double depth = fx * b / (disparity.at<float>(v, u));
            // 计算摄像头坐标系下的点坐标
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

代码地址：
https://github.com/gaoxiang12/slambook2.git 高翔slam_demo ch5

参考资料

1.《视觉SLAM十四讲》
2.https://zhuanlan.zhihu.com/p/340751380 鱼眼相机模型
3.https://blog.csdn.net/weixin_41484240/article/details/80500903 射影变换，仿射变换，透视变换
4.https://blog.csdn.net/xinxiangwangzhi_/article/details/103367957 透视变换与射影变换