本文为视觉 SLAM 学习总结,讲解观测方程中 z k , j z_{k,j} zk,j​ 的具体形式。欢迎交流

本讲内容概要

  • 针孔相机的模型、内参与径向畸变参数
  • 空间点如何投影到相机成像平面的
  • OpenCV 的图像存储与表达方式
  • 基本的摄像头标定方法

相机模型

照片只能记录物体在成像平面上的投影,无法测得“距离”维度的信息。而普通相机可以用针孔模型进行很好地近似。

小孔成像模型

小孔成像的原理图如下:

在这里插入图片描述

左图为立体图像,在相机平面建立三维坐标系,在像素平面建立二维坐标系。则空间中一点 P P P 透过小孔投影到成像平面的坐标可以计算。右图为俯视图,由相似三角形有,小孔成像模型的原始形式为:
Z f = − X X ′ = − Y Y ′ \frac{Z}{f}=-\frac{X}{X'}=-\frac{Y}{Y'} fZ​=−X′X​=−Y′Y​
其中负号表示为倒像。相机内部会将图像翻转,因此我们平时看到的是正像。若将像翻转到前面则可消除负号:
Z f = X X ′ = Y Y ′ \frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'} fZ​=X′X​=Y′Y​
整理后得到:
X ′ = f X Z X'=f\frac{X}{Z} X′=fZX​

Y ′ = f Y Z Y'=f\frac{Y}{Z} Y′=fZY​

成像平面的单位是米,而像素坐标的单位为像素,两种表示方法有一定的比值和中心移动。因此将成像平面到像素坐标:
{ u = α X ′ + c x v = β Y ′ + c y \left\{\begin{aligned}u & = & αX'+c_x \\v & = & βY'+c_y \\\end{aligned}\right. {uv​==​αX′+cx​βY′+cy​​
将其带入到 X ′ , Y ′ X',Y' X′,Y′ 得:
{ u = f x X Z + c x v = f y Y Z + c y \left\{\begin{aligned}u & = & f_x\frac{X}{Z}+c_x \\v & = & f_y\frac{Y}{Z}+c_y \\\end{aligned}\right. ⎩⎪⎪⎨⎪⎪⎧​uv​==​fx​ZX​+cx​fy​ZY​+cy​​
其中 f x = α f , f y = β f f_x=αf,f_y=βf fx​=αf,fy​=βf。有 4 个参数 u , v , c x , c y u,v,c_x,c_y u,v,cx​,cy​,一般 f x , f y f_x,f_y fx​,fy​ 大小近似, c x , c y c_x,c_y cx​,cy​ 在中心附近。将上式写为矩阵的形式:

在这里插入图片描述

其中等式左边为齐次坐标。 K K K 称为内参矩阵,相机生产出后内参固定。当对图像进行缩放、剪切等操作时,内参矩阵也会相应产生变化,可以用上面的公式推出。

传统习惯写法,也称为投影方程。:

在这里插入图片描述

因为左边为齐次坐标,将其进行比例缩放后表示意思是相同的。下图中若已知成像坐标, P P P 可以在射线上的任意一点,即 P P P 的坐标可以乘一个比例,无法知道具体距离。 反之亦然。

在这里插入图片描述

除了内参外,相机坐标系还和世界坐标系相差了一个变换:

在这里插入图片描述

这里的 R , t R,t R,t 或 T T T 为相机在世界坐标下的位姿,即外参,是 SLAM 估计的目标。右侧式子隐藏了非齐次到齐次的变换。

坐标变换的整个过程为:世界→相机→归一化平面→像素。

在这里插入图片描述

畸变

相机中的透镜会对图像产生畸变,不满足小孔成像模型。

主要的畸变类型:径向畸变和切向畸变

  • 径向畸变:畸变大小与像素离中心的距离有关

    在这里插入图片描述

    桶形失真将径向距离变大,枕形失真将距离变小。

  • 切向畸变:畸变大小与点的夹角有关

    在这里插入图片描述

    相机感光器件没有垂直于光心,有较小的偏差角。

数学模型

畸变可以用归一化坐标的变换来描述:

  • 径向畸变,多项式描述:
    x c o r r e c t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) x_{corrected}=x(1+k_1r^2+k_2r^4+k_3r^6) xcorrected​=x(1+k1​r2+k2​r4+k3​r6)

    y c o r r e c t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y_{corrected}=y(1+k_1r^2+k_2r^4+k_3r^6) ycorrected​=y(1+k1​r2+k2​r4+k3​r6)

    其中 r 2 = x 2 + y 2 r^2=x^2+y^2 r2=x2+y2。

  • 切向畸变,使用交叉项描述:
    x c o r r e c t e d = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) x_{corrected}=x+2p_1xy+p_2(r^2+2x^2) xcorrected​=x+2p1​xy+p2​(r2+2x2)

y c o r r e c t e d = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y y_{corrected}=y+p_1(r^2+2y^2)+2p_2xy ycorrected​=y+p1​(r2+2y2)+2p2​xy

放在一起得:
x c o r r e c t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) x_{corrected}=x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2) xcorrected​=x(1+k1​r2+k2​r4+k3​r6)+2p1​xy+p2​(r2+2x2)

y c o r r e c t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y y_{corrected}=y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy ycorrected​=y(1+k1​r2+k2​r4+k3​r6)+p1​(r2+2y2)+2p2​xy

小结

总结一下小孔成像的步骤:

  1. 世界坐标系下有一点 P P P,世界坐标为 P w P_w Pw​
  2. 由于相机在运动, P P P 的相机坐标为: P c ~ = R P w + t \widetilde{P_c}=RP_w+t Pc​ ​=RPw​+t。
  3. 这时 P c ~ \widetilde{P_c} Pc​ ​ 仍有 X , Y , Z X,Y,Z X,Y,Z 三个量,将其投影到归一化平面 Z = 1 Z=1 Z=1 上,得到 P P P 的归一化相机坐标: P c = [ X / Z , Y / Z , 1 ] T P_c=[X/Z,Y/Z,1]^T Pc​=[X/Z,Y/Z,1]T。
  4. P P P 的归一化坐标经过内参后,对应到像素坐标: P u v = K P c P_{uv}=KP_c Puv​=KPc​

双目相机模型

双目相机实为两个单目相机,两个眼的位姿相同,之间的距离称为基线。类似人眼。

在这里插入图片描述

右侧几何模型中两个三角形相似,因此得到左右像素的几何关系:
z − f z = b − u L + u R b \frac{z-f}{z}=\frac{b-u_L+u_R}{b} zz−f​=bb−uL​+uR​​
整理得:
z = f b d , d = u L − u R z=\frac{fb}{d},\quad d=u_L-u_R z=dfb​,d=uL​−uR​
其中 d d d 称为视差。因为 d d d 最小为一个像素,因此可以测量的距离有最大值: f b fb fb。 d d d 不易测得,很难知道两个像素对应的是同一个点。

由此可由左右点像素的差异推出 P P P 的距离,这也是双目相对于单目的特点。

RGB-D 相机模型

通过物理手段测量每一个 RGB 像素的深度,而不是像双目一样进行计算。根据原理的不同可分为两类:

  • 结构光:通过反射回的光的结构计算距离,还是要计算

  • ToF:通过计算脉冲间的时间差,和激光类似,效果更好。

在这里插入图片描述

问题:

  • 光会被透射或吸收:玻璃墙或透射、吸收光材质的距离无法测量。
  • 红外光在户外易受太阳光或其他红外光的干扰。

图像

图像在计算机中以矩阵形式存储(二维数组),原点在左上角。需要对感光度量乘数据(彩色图像还有通道——红绿蓝):

在这里插入图片描述

实践:基本图像处理

讲解 OpenCV 的使用。这里有 OpenCV 的安装。

当有多个 OpenCV 版本时,可以在 CMakeLists.txt 中指定需要使用的版本:

# 寻找OpenCV库
find_package( OpenCV 3.1 REQUIRED )

OpenCV 用 cv::Mat 表示图像,实际是矩阵:

// 读取argv[1]指定的图像
cv::Mat image;
//cv::imread函数读取指定路径下的图像,为第一个参数
image = cv::imread ( argv[1] );
// 判断图像文件是否正确读取
if ( image.data == nullptr ) {// 数据不存在,可能是文件不存在
	cerr<<"文件"<<argv[1]<<"不存在."<<endl;
	return 0;
}

文件顺利读取,首先输出一些基本信息:

cout<<"图像宽为"<<image.cols<<",高为"<<image.rows<<",通道数为"<<image.channels()<<endl;
cv::imshow ( "image", image );      // 用cv::imshow显示图像
cv::waitKey ( 0 );                  // 暂停程序,等待一个按键输入
// 判断image的类型
if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 ){
    // 图像类型不符合要求
    cout<<"请输入一张彩色图或灰度图."<<endl;
    return 0;
}

接下来遍历图像,OpenCV 一般按照指针和偏移量的方式来访问图像,若直接调用接口效率较低:

// 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
// 使用 std::chrono 来给算法计时
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for ( size_t y=0; y<image.rows; y++ ){
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char* row_ptr = image.ptr<unsigned char> ( y ); // row_ptr是第y行的头指针
    for ( size_t x=0; x<image.cols; x++ ){
        // 访问位于 x,y 处的像素
        unsigned char* data_ptr = &row_ptr[ x*image.channels() ];// data_ptr 指向待访问的像素数据
        // 输出该像素的每个通道,如果是灰度图就只有一个通道
        for ( int c = 0; c != image.channels(); c++ )
            unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
    }
}
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"遍历图像用时:"<<time_used.count()<<" 秒。"<<endl;

关于 cv::Mat 的拷贝:

// 直接赋值并不会拷贝数据,是浅拷贝,作引用
cv::Mat image_another = image;
// 修改 image_another 会导致 image 发生变化
image_another ( cv::Rect ( 0,0,100,100 ) ).setTo ( 0 ); // 将左上角100*100的块置零
cv::imshow ( "image", image );
cv::waitKey ( 0 );

// 使用clone函数来拷贝数据,深拷贝
cv::Mat image_clone = image.clone();
image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 );
cv::imshow ( "image", image );
cv::imshow ( "image_clone", image_clone );
cv::waitKey ( 0 );

// 删除所有窗口
cv::destroyAllWindows();

对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看 OpenCV 官方文档查询每个函数的调用方法。

实践:点云拼接

为了进一步理解内外参。我们准备了 5 张彩图和 5 张对应的深度图,有了 color 和 depth 我们就可以算每个像素在相机坐标系下的位置。

在这里插入图片描述

在这里插入图片描述

我们还准备了一个 pose.txt,里面有 5 个图像对应的相机位姿,前三列为平移,后四列为四元数,四元数的实部在后面:

-0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042
-0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662
-0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536
-1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178
-1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741

有了相机位姿,我们就可以将图像的位姿转换到世界坐标系中,将图像中的点都加起来,就得到了一个全局的地图(点云)。

相机内参直接在程序中给定:

// 相机内参 
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

首先读取图像:

vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿

ifstream fin("./pose.txt");
if (!fin)
{
    cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
    return 1;
}
for ( int i=0; i<5; i++ )
{
    boost::format fmt( "./%s/%d.%s" ); //图像文件格式
    colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
    depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

    double data[7] = {0};
    for ( auto& d:data )
        fin>>d;
    Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );// 旋转构造四元数
    Eigen::Isometry3d T(q);
    T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));// 平移
    poses.push_back( T );// 变换矩阵放入vector
}

然后计算每个点的世界坐标:

// 定义点云使用的格式:这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT; 
typedef pcl::PointCloud<PointT> PointCloud;

// 新建一个点云
PointCloud::Ptr pointCloud( new PointCloud ); 
for ( int i=0; i<5; i++ )
{
    cout<<"转换图像中: "<<i+1<<endl; 
    cv::Mat color = colorImgs[i]; 
    cv::Mat depth = depthImgs[i];
    Eigen::Isometry3d T = poses[i];
    for ( int v=0; v<color.rows; v++ )
        for ( int u=0; u<color.cols; u++ )
        {            
            unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
            if ( d==0 ) continue; // 为0表示没有测量到
            Eigen::Vector3d point; 
            // 先计算相机坐标系下的坐标,知道u,v求X,Y,Z
            point[2] = double(d)/depthScale; 
            point[0] = (u-cx)*point[2]/fx;
            point[1] = (v-cy)*point[2]/fy; 
            // 得到世界坐标系下的坐标
            Eigen::Vector3d pointWorld = T*point;
			
            // 将坐标放入点云
            PointT p ;
            p.x = pointWorld[0];
            p.y = pointWorld[1];
            p.z = pointWorld[2];
            p.b = color.data[ v*color.step+u*color.channels() ];
            p.g = color.data[ v*color.step+u*color.channels()+1 ];
            p.r = color.data[ v*color.step+u*color.channels()+2 ];
            pointCloud->points.push_back( p );
        }
}

安装 PCL

建立点云需要用到 PCL,Ubuntu 16.04 只需要执行下面的命令:

sudo apt-get install libpcl-dev pcl-tools

还需要在 CMakeLists.txt 中加入下面的语句,删除一个不存在的库:

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu16.04

并且还要安装一个库,提供必需的 libproj.so:

sudo apt-get install libproj-dev

运行

在 pose.txt 目录下运行,得到 map.pcd 点云文件,共有 108w 个点:

在这里插入图片描述

用 pcl_viewer 打开点云:

pcl_viewer map.pcd

得到点云生成的场景,可以 360° 的旋转:

在这里插入图片描述

因为相机的位姿以及在 pose.txt 中给出,因此该问题难度较小。由于深度图像存在噪声,因此在沿着深度相机观察的直线上会有很多波浪形噪声,距离越远噪声越大。

至此我们相机与图像的知识就介绍到这里,你已经可以用点云得到三维图像了,可以自己动手拍一些照片来实践一下啦。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐