5.4 实践：拼接点云

一、安装PCL库

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl //加入源
按enter建
sudo apt-get update //更新，这一步可能报错，原因是国内源
————————————————————————————————
使用国内源按教材命令容易报错，解决见：

http://www.cnblogs.com/fudong071234/p/6359725.html

编译安装见：

http://www.linuxdiyf.com/linux/24123.html
————————————————————————————————

************************************分割线***************************************

——————————————————————————————
教材给出指令：
sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl

sudo apt-get update

sudo apt-get install libpcl-all

——————————————————————————————

其中最后一条指令可能报错，执行下列指令：

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl

sudo apt-get update

sudo apt-get install libpcl1.7

二、程序测试
代码注释可以参考这个人的博客：
http://www.mamicode.com/info-detail-2180344.html
—————————————分割线—————————————
1.预备知识

首先是vector的用法:
http://www.cnblogs.com/wang7/archive/2012/04/27/2474138.html
——————————————————————————————
Eigen库中各种形式的表示如下：
旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1）:Eigen::AngleAxisd
四元数（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d
——————————————————————————————
push_back 方法介绍:
vector::void push_back (const value_type& val);
vector::void push_back (value_type&& val);
该函数将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素，新的元素的值是val的拷贝（或者是移动拷贝）
——————————————————————————————
cv::imread( , )第二个参数含义：
https://blog.csdn.net/z914022466/article/details/52709981
——————————————————————————————
if(!a) //如果a=0，！a=1，继续操作
——————————————————————————————
for(auto& x:y):

//for循环中的每个迭代初始化一个新的引用
for (auto &c : s)   
    c = toupper(c); 
//相当于
for (auto it = s.begin(); it != s.end(); ++it)
{
    auto &c = *it;
    c = toupper(c);
}

—————————————分割线—————————————
2.代码及注释：

#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;    // 彩色图和深度图
vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿

ifstream fin("./pose.txt");
//cout<<"fin="<<fin<<endl;
if (!fin) //如果为真继续操作，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};
//cout<<"data="<<data[7]<<endl;
    for ( auto& d:data )
        fin>>d;                      //fin给data[]
    Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
    Eigen::Isometry3d T(q);          //4*4
    T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
    poses.push_back( T );
//cout<<"pose="<<q<<endl;   //讲pose文件里的东西读入data数组中
}

// 计算点云并拼接
// 相机内参 
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;    //为什么要除以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 );
        }
}

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

————————————————————————————————
会生成地图文件map.pcd
进入存储文件的目录后可以用如下指令读取：
pcl_viewer map.pcd
若需要安装软件，指令如下：
sudo apt install pcl-tools