视觉slam十四讲ch5 5.4 实践：拼接点云

0. 坐标系说明

1.世界坐标->相机坐标

 

 2.相机坐标->物理坐标

根据相似三角形原理(负号代表倒立)：

将成像平面对称到相机前方：

 

 整理得到：

写成齐次坐标：

 3.相机坐标->物理坐标

dx,dy表示：x方向和y方向的一个像素分别占多少个（可是小数）长度单位。

u0,v0表示：图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数。

 物理成像坐标和像素坐标转化(m和pix的转化)：

 

 写成齐次坐标：

 4.像素坐标->物理坐标->相机坐标->世界坐标

简化：

由于齐次坐标乘上非零常数表示同样的含义，所以可以去掉Z（注意：在齐次坐标下有意义）：

所以：只要知道一个像素点[u, v]且知道该像素点对应的P在相机坐标系下的深度Zc，那么，再结合旋转平移矩阵，即可得到世界坐标系下的(Xw,Yw,Zw)。

5.实例

高翔博士在《视觉slam十四讲》的ch5中提供了5张 RGB-D图像，并且知道每幅图像的内参和外参。根RGB-D图像和相机内参，我们可以计算任何一个像素在相机坐标系下的坐标（位置）。同时，根据相机的位姿 ，又能计算这些像素在世界坐标系下的坐标（位置）。如果把所有像素的空间坐标（世界坐标系）都求出来，相当于构建一张类似于地图的东西。

joinMap.cpp

#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( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // colorImgs：彩色图；depthImgs：深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;   // 相机位姿
    
	//iftream的对象假设为fin，fin在读取数据的时候会根据你的输出对象来选择输出的方式。
    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() ));
		/*
		cv::Mat img =cv::imread(argv[1],-1)
		函数原型Mat imread( const String& filename, int flags = IMREAD_COLOR );
		第一个参数是图片的绝对地址;
		第二个参数表示图片读入的方式(flags可以缺省，缺省时flags=1，表示以彩色图片方式读入图片);
		flags>0时表示以彩色方式读入图片;
		flags=0时表示以灰度图方式读入图片;
		flags<0时表示以图片的本来的格式读入图片;
		*/
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
        
        double data[7] = {0};
        for ( auto& d:data )
            fin>>d; //将深度值文件一行一行读进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 );
    }
    
    // 计算点云并拼接
    // 相机内参 
    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]; // 深度值
				/*
				d==0:表示该像素点没有深度值(不可能)，所以就抛弃该点，不再计算相机坐标系下的坐标值(X,Y,Z)
				*/
                if ( d==0 ) continue; // 为0表示没有测量到
				
                //point:相机坐标系下的坐标值(X,Y,Z)
				Eigen::Vector3d point; 
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
				
				// pointWorld：世界坐标
                Eigen::Vector3d pointWorld = T*point;
                
				// p：点云(每个点云按照[XYZRGB]的格式表示)
                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;
}

CMakeLists.txt

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

在运行过程中可能报错：

error: #error PCL requires C++14 or above

解决方法：修改CMakeLists.txt：

在CMakeLists.txt中添加：set( CMAKE_CXX_STANDARD 14) 

cmake_minimum_required( VERSION 2.8 )
project( joinMap )

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

set( CMAKE_CXX_STANDARD 14) #huarzail：add the CMAKE_CXX_STANDARD

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

编译之后会生成：joinMap可执行文件

 运行之后，会生成map.pcd点云文件：

想要查看map.pcd点云文件，需要先安装pcl-tools：

sudo apt-get install  pcl-tools

然后使用pcl_viewer命令显示点云图。

pcl_viewer map.pcd

点云拼接结果。

 参考文献：

视觉SLAM十四讲 全网电子书-高翔