视觉SLAM十四讲读书编程笔记 Chapter5 相机与图像

关注

原创最新推荐文章于 2026-03-20 00:16:20 发布 1.1k 阅读 AI 写同款· GEO 优化 ›

视觉SLAM十四讲读书笔记此为视觉SLAM十四讲第二章的读书笔记 1、P24 第二章对于SLAM问题的由来以及建模的发展演化过程有一个比较清晰的介绍（想吐槽一下的就是：在一个研究领域刚兴起的时候，真的就是百花齐放啊，行业内对于同一个东西会有各种各样的描述，虽然每个作者的描述都没有错，但是对于初学者而言，今天在文章中看到一个“状态估计的不确定性研究”，明天看到一个“CML”或者“SLAM”，但是其实描述的是同一个东西，徒增了一些困惑），特别是对于初学者，对于SLAM问题到底是个啥？描述的很通俗易懂。 2、Ubuntu的安装为了减少不阅读详情

相机模型

使用针孔和畸变两个模型来描述相机的整个投影过程，把外部的三维点投影到相机内部的成像平面，就构成了相机的内参数。
针孔相机模型

在相机坐标系下，设物点P坐标为[X,Y,Z]^T,像点P’坐标为[X’,Y’,Z’]^T,显然Z’=f，那么根据相似三角形，有：
Z/f = -X/X’ = -Y/Y’
这是初中物理的知识，其中负号表示像是倒着的，像的正倒对于CCD传感器没有任何实际意义，因此将负号去掉：
Z/f = X/X’ = Y/Y’，整理得：X’=fX/Z Y’=fY/Z
内参矩阵：像素坐标系与物理成像平面坐标系互相转换
像素坐标系定义：原点位于左上角，u轴向右与x轴平行，v轴向下与y轴平行。
像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。
设像素坐标在u轴上缩放了alpha倍，在v轴上缩放了beta倍，原点平移了[c_x,c_y]^T
像点P’与像素坐标[u,v]^T的关系为：
u = alpha X’ + c_x
v = beta X’ + c_y
令f_x = alpha f , f_y = beta f ,那么：
u = f_xX/Z + c_x
v = f_yY/Z + c_y
写成矩阵形式为：
按照传统习惯，将Z挪到左侧：

其中K就是相机的内参矩阵，它与焦距f，原点的平移[c_x,c_y]^T,u轴上的缩放alpha，v轴上的缩放beta有关。
内参矩阵&外参矩阵：世界坐标系向像素坐标系的转换

这里的P_w=[X_w,Y_w,Z_w,1]^T,是齐次坐标；
TP_w=[X_c,Y_c,Z_c,1]^T,表示的也是齐次坐标；
这里TP_w隐含了一次齐次坐标向非其次坐标转换的过程，转换完之后，不区分记号P_c=TP_w=[X_c,Y_c,Z_c]^T；
如果将P_c也进行归一化，上图中式子两边同除Z(实质上是Z_c)，就变成了齐次坐标等式：P_uv=KTP_w ,
此时P_c=[X_c/Z_c,Y_c/Z_c,1]^T,成为归一化坐标，它位于相机前方z=1处的平面上，该平面也被称为归一化平面。
径向畸变
径向畸变是由透镜形状引入的，分为桶形失真和枕形失真。
设[x,y]^T是归一化的平面点坐标，[x_distorted,y_distorted]^T是畸变后点的坐标。
x_distorted = x(1 + k₁r² + k₂r⁴ + k₃r⁶)
y_distorted = y(1 + k₁r² + k₂r⁴ + k₃r⁶)
畸变较小的图像中心区域，畸变纠正主要是k₁在起作用；而对于畸变较大的边缘区域，主要是k₂在起作用。普通相机用这两个系数就可以很好的纠正径向畸变，对于畸变很大的摄像头，比如鱼眼镜头，可以加入k₃畸变项对径向畸变进行纠正。
切向畸变
切向畸变是由于透镜和成像平面不严格平行而引入的。

切向畸变可以用两个参数p₁，p₂来进行纠正：
x_distorted = x + 2p₁xy + p₂(r²+2x²)
y_distorted = y + 2p₂xy + p₁(r²+2y²)
径向畸变和切向畸变共同作用
假设相机坐标系中有一点P(X,Y,Z),通过径向畸变和切向畸变的五个系数，可以找到这个点在像素平面上的正确位置：
(1)将相机坐标系下的三维点投影到归一化图像平面。设它的归一化坐标为[x,y]^T.
(2)对归一化平面上的点进行径向畸变和切向畸变纠正。给定归一化坐标，可以求出原始图像上的坐标
x_distorted = x(1 + k₁r² + k₂r⁴ + k₃r⁶) + 2p₁xy + p₂(r²+2x²)
y_distorted = y(1 + k₁r² + k₂r⁴ + k₃r⁶) + 2p₂xy + p₁(r²+2y²)
(3)将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
u = f_xx_distorted + c_x
v = f_yy_distorted + c_y

实际使用中，存在两种去畸变处理方法。
方法一：先对整张图像进行去畸变处理，得到去畸变之后的图像，然后用针孔模型讨论空间点的投影问题。
方法二：考虑图像中的某个点，按照去畸变的方程，讨论其去畸变后的空间位置。

双目相机原理简介

在这里插入图片描述
如图所示，左眼像素P_L(u_L,v_L),右眼像素P_R(u_R,v_R).
假设左眼像素和右眼像素的v是相同的，即相机只在水平方向发生旋转或平移。那么沿着x方向O_LP_L的距离为u_L - c_x,沿着x方向O_RP_R的距离为c_x - u_R
根据三角形PP_LP_R和三角形PO_LO_R的相似关系，有：
(z-f)/z = (b-(u_L-c_x)-(c_x-u_R))/b
(z-f)/z = (b-(u_L-u_R))/b
设视差d=u_L-u_R，那么：
(z-f)/z = (b-d)/b
整理得：
z = fb/d
可见，视差与距离成反比：视差越大，距离越近。
而又因为视差最小为一个像素，所以，双目深度存在一个极限值，由fb决定。fb越大，设备能测到的距离就越远。

实践：图像的存取与访问

安装opencv的依赖

sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev

在ubuntu16.04上出现报错
在这里插入图片描述
按照提示改成：

sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff5-dev libjasper-dev libopenexr-dev libtbb-dev

opencv源码编译
opencv本身也是一个cmake工程，make之后，要调用如下命令将其安装在机器上

sudo make install

一些问题说明
我下载的opencv源码版本为opencv-3.3.0.zip，使用的操作系统是ubuntu16.04
在执行make时，出现如下报错：
.
解决办法如下：

sudo apt-get autoremove libtiff5-dev
sudo apt-get install libtiff5-dev

然后再make就可以成功了。

代码：
imageBasics,cpp

//2019.08.07
#include <iostream>
#include <chrono>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char** argv)
{
	//读取argv[1]指定的图像
	cv::Mat image;
	image = cv::imread(argv[1]);//cv::imread函数读取指定路径下的图像
	//判断图像文件是否正确读取
	if(image.data == nullptr)//数据不存在，可能是文件不存在
	{
		cerr << "文件" << argv[1] << "不存在." << endl;
		return 0;
	}
	
	//若文件读取顺利，首先输出一些基本信息
	cout<<"图像宽为"<<image.cols<<", 高为"<<image.rows<<", 通道数为"<<image.channels()<<endl;
	cv::imshow("image",image);//用cv::imshow()显示图像
	//判断image的类型
	if(image.type() != CV_8UC1 && image.type() != CV_8UC3)
	{
		//图像类型不符合要求
		cout << "请输入一张彩色图或灰度图."<<endl;
		return 0;
	}
	
	//遍历图像
	//使用std::chrono来给算法计时
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	for( size_t y=0; y<image.rows;y++)
	{
		for( size_t x=0;x<image.cols;x++)
		{
			//访问位于x，y处的像素
			//用cv::Mat::ptr获得图像的行指针
			unsigned char* row_ptr = image.ptr<unsigned char>(y);//row_ptr是第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();
	return 0;


}

CMakeLists.txt

#2019.08.07
# 添加C++标准文件
set(CMAKE_CXX_FLAGS "-std=c++11")

#寻找opencv库
find_package(OpenCV REQUIRED)

#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})


add_executable(imageBasics imageBasics.cpp)

#链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})

运行结果：
在这里插入图片描述

实践：点云拼接

安装PCL

PCL安装命令

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-all

在ubuntu16.04上运行报错，原因是源被移除。

解决办法：下载PCL源码，手动编译安装,步骤如下
(1)从github上下载pcl源码，下载地址为https://github.com/PointCloudLibrary/pcl
下载完成后，得到pcl-master.zip文件，解压

(2)安装相应依赖库

    sudo apt-get update
    sudo apt-get install git build-essential linux-libc-dev
    sudo apt-get install cmake cmake-gui 
    sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev
    sudo apt-get install mpi-default-dev openmpi-bin openmpi-common  
    sudo apt-get install libflann1.8 libflann-dev
    sudo apt-get install libeigen3-dev
    sudo apt-get install libboost-all-dev
    sudo apt-get install libvtk5.10-qt4 libvtk5.10 libvtk5-dev
    sudo apt-get install libqhull* libgtest-dev
    sudo apt-get install freeglut3-dev pkg-config
    sudo apt-get install libxmu-dev libxi-dev 
    sudo apt-get install mono-complete
    sudo apt-get install qt-sdk openjdk-8-jdk openjdk-8-jre

(2)在pcl-master文件夹下新建文件夹build
(3)在build文件夹下编译

cmake  -DCMAKE_BUILD_TYPE=None  -DCMAKE_INSTALL_PREFIX=/usr -DBUILD_GPU=ON  -DBUILD_apps=ON  -DBUILD_examples=ON  -DCMAKE_INSTALL_PREFIX=/usr ..
make -j2  
sudo make install

(4)可选，PCL可视化相关包安装

sudo apt-get install libopenni-dev 
sudo apt-get install libopenni2-dev

大功告成!

数据说明

在slambook/ch5/joinMap中准备了5对图像，在color/下有1.png到5.png共5张RGB图，而在depth/下有5张对应的深度图。同时，pose.txt文件给出了5张图像相机位姿(以T_WC形式)。位姿记录的形式是平移向量加旋转四元数：[x,y,z,q_x,q_y,q_z,q_w]

点云拼接

(1)根据内参计算一对RGB-D图像对应的点云。
(2)根据各张图的相机位姿，把点云加起来，组成地图。
完整代码：
joinMap.cpp

//2019.08.08
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>//for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
//#include <pcl/visualization/pcl_visualizer.h>//注释掉，因为没有编译显示相关的包

int main()
{
	vector<cv::Mat> colorImgs, depthImgs;	//彩色图和深度图
	vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;//相机位姿(欧式变换矩阵4×4)
	
	ifstream fin("../pose.txt");
	if(!fin)
	{
		cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
		return 1;
	}

	//读取5组彩色图、深度图和相机位姿
	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;
		
		//构造位姿矩阵T		
		Eigen::Quaterniond q(data[6],data[3],data[4],data[5]);//文件中位姿的记录形式是[x,y,z,qx,qy,qz,qw],而Eigen构造的四元数要把实部放在最前面
		Eigen::Isometry3d T(q);//旋转用四元数表示
		T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2]));//设置平移向量
		
		poses.push_back(T);
	}
	
	//计算点云并拼接
	//相机内参
	double cx = 325.5;
	double cy = 253.5;
	double fx = 518.0;
	double fy = 519.0;
	double depthScale = 1000.0;

	cout<<"正在将图像转换为点云..."<<endl;
	//定义点云使用的格式，这里用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;
				point[2] = double(d)/depthScale;
				point[0] = (u-cx)*point[2]/fx;
				point[1] = (v-cy)*point[2]/fy;
				Eigen::Vector3d pointWorld = T*point;//这里的T表示的是Twc,是将相机坐标系转换为世界坐标系的外参矩阵

				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);
				
			}
		}
	}


	pointCloud->is_dense = false;
	cout <<"点云共有"<<pointCloud->size()<<"个点."<<endl;
	pcl::io::savePCDFileBinary("map.pcd",*pointCloud);


	return 0;

}

值得注意的是，必须注释掉头文件的最后一行，因为在上面pcl的安装中没有编译显示相关的包。
//#include <pcl/visualization/pcl_visualizer.h>
CMakeLists.txt

#2019.08.29
cmake_minimum_required( VERSION 2.8 )
project( joinMap )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# opencv 
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# eigen 
include_directories( "/usr/include/eigen3/" )

# pcl 
find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )

add_executable( joinMap joinMap.cpp )
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )

运行结果：
编译运行之后，在build文件夹下会生成map.pcd文件。
然后，输入如下命令显示这个map.pcd文件：