《视觉SLAM十四讲精品总结》3：OpenCV 与点云图

一、OpenCV 图像

灰度图中，用0-255的整数表示灰度大小，一张宽度为640像素，高度为480像素分辨率的灰度图表示为：

unsigned char image[480][640]

二维数组，先行后列

访问图像中某个像素，需要指明他的坐标，灰度值 I(x,y)的读数

unsigned char pixel=image[y][x]

遍历图像：

for(int y=0;y<image.rows;y++)
{
   for(int x=0;x<image.cols;x++)
    {
        //row_ptr是第y行的头指针
        unsigned char* row_ptr=image.ptr<unsigned char> (y);
        //data_ptr指向待访问的像素数据
        unsigned char* data_ptr=&row_ptr[x*image.channels()];

        //输出像素的每个通道
        for(int c-0;c!=image.channels();c++)
         {
            unsigned char data=data_ptr[c];
         }
    }
}

彩色图像通道的默认顺序是B、G、R。

1、imageBasics.cpp

#include <iostream>
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;

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显示图像
    cv::waitKey ( 0 );                  // 暂停程序,等待一个按键输入
    return 0;
}

2、CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( imageBasics )

# 寻找OpenCV库
find_package( OpenCV REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )
# 可执行程序
add_executable( imageBasics imageBasics.cpp )
# 链接OpenCV库
target_link_libraries( imageBasics ${OpenCV_LIBS} )

调用：build/imageBasics ubuntu.png

二、拼接点云

通过点云拼接，我们就可以还原这个房间的三维场景。

已知：5张RGB-D图像，每个图像的内参K和外参T

目标：计算所有像素在世界坐标系的位置，把点云加起来，组成地图。

思路：根据pose.txt中相机外参（平移向量+旋转四元数）转换成变换矩阵T（4*4）；对相机坐标（根据像素和实物关系得到）通T转换成世界坐标；之后根据5张图循环构造点云。

Step1：读取RGB图片和深度图片，以及相机位姿数据。

int main(int argc, char** argv)
{
	// 放入容器vector中
	// 彩色图和深度图，OpenCV 库里的Mat类
	// 相机位姿，Eigen 库里的Isometry3d变换矩阵T（4*4）类
	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;
	}
	//外参变换矩阵T（4*4）
	for (int i = 0; i<5; i++)
	{
		//boost::format 格式化字符串  拼接出图片文件名
		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]);
		//变换矩阵T初始化旋转部分
		Eigen::Isometry3d T(q);
		//T初始化平移向量部分
		T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
		//存储T到位姿数组中
		poses.push_back(T);
	}

需要强调的关键点：

• 设地址：位姿地址只有一个(./pose.txt),但图片地址有多个，需要在for循环里把5张图格式统一下。
• 转换变换矩阵T：取出相机位姿(./pose.txt)，包括用四元数表示的旋转和xyz轴平移，保存到变换矩阵T（4*4）中。

Step 2：设定相机内参

相机内参K用来将图片中的像素点转换到相机坐标系，进而再使用变换矩阵T变换到世界坐标系。

  

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

 Step 3：拼接点云

pcl点云库提供了非常方便的调用接口，只需要传入每个点的三维坐标和颜色，就可以把多张图片自动拼接到一起。

像素坐标系到相机坐标系

	//定义RGB值pointT和点云pointCloud类
	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;
		//颜色、深度、位姿T
		cv::Mat color = colorImgs[i];
		cv::Mat depth = depthImgs[i];
		Eigen::Isometry3d T = poses[i];
		//已知像素坐标，遍历所有像素（u,v）
		for (int v = 0; v<color.rows; v++)
			for (int u = 0; u<color.cols; u++)
			{
			// 深度值d
			unsigned int d = depth.ptr<unsigned short>(v)[u]; 
			if (d == 0) continue; // 为0表示没有测量到
			//像素坐标(u,v,d)计算相机坐标系下坐标 point
			Eigen::Vector3d point;
			point[2] = double(d) / depthScale;
			point[0] = (u - cx)*point[2] / fx;
			point[1] = (v - cy)*point[2] / fy;
			//相机位姿T计算在世界坐标系下坐标 pointWorld
			Eigen::Vector3d pointWorld = T*point;

			//pcl点 pointT ，x,y,z,b,g,r
			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];
			//push_back(p)放进去一个个点p，构成了点云pointCloud
			pointCloud->points.push_back(p);
			}
	}

	pointCloud->is_dense = false;
	cout << "点云共有" << pointCloud->size() << "个点." << endl;
	//拼接点云，点云是指针形式
	pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
	return 0;

 

• 世界坐标系下的点是用Eigen::Vector3d保存的，而点云中的点是用PointT保存的，它们并不兼容。

CMakeLists.txt

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

可视化

pcl_viewer map.pcd