【视觉SLAM十四讲】相机与图像(附PCL安装教程)
本文为视觉 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==fxZX+cxfyZY+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+k1r2+k2r4+k3r6)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+k1r2+k2r4+k3r6)
其中 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+2p1xy+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)+2p2xy
放在一起得:
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+k1r2+k2r4+k3r6)+2p1xy+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+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
小结
总结一下小孔成像的步骤:
- 世界坐标系下有一点 P P P,世界坐标为 P w P_w Pw
- 由于相机在运动, P P P 的相机坐标为: P c ~ = R P w + t \widetilde{P_c}=RP_w+t Pc =RPw+t。
- 这时 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。
- 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 中给出,因此该问题难度较小。由于深度图像存在噪声,因此在沿着深度相机观察的直线上会有很多波浪形噪声,距离越远噪声越大。
至此我们相机与图像的知识就介绍到这里,你已经可以用点云得到三维图像了,可以自己动手拍一些照片来实践一下啦。
更多推荐
所有评论(0)