《视觉slam十四讲》初学小白笔记（5）

 拼接点云

#include<iostream>
#include<fstream>
using namespace std;
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<Eigen/Geometry>
#include<boost/format.hpp>
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include<pcl/visualization/pcl_visualizer.h>

int main(int argc,char** argv){
    //读取彩色和深度图像对和位姿信息[x,y,z,qx,qy,qz,qw],其中qw为实部，并把位姿从四元数与平移向量转化成变换矩阵
    vector<cv::Mat> colorImgs,depthImgs;		//彩色图和深度图
    
    //Eigen::Isometry3d三维变换矩阵（实际是4*4矩阵）
    vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;//相机位姿
    
    ifstream fin("./pose.txt");		//文件读操作，存储设备读区到内存中
    if(!fin){
      cerr<<"请在有pose.txt的目录下运行此程序"<<endl;	//cerr：输出到标准错误的ostream对象，常用于程序错误信息；
      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]);
      Eigen::Isometry3d T(q); //将四元数转成变换矩阵的旋转部分
      T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2]));//T初始化平移向量部分
      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];
      //遍历所有像素（u，v）
      for(int v=0;v<color.rows;v++){
	for(int u=0;u<color.cols;u++){
	  //opencv中mat数据类型指针ptr
	  unsigned int d=depth.ptr<unsigned short>(v)[u];//深度   表示指向depth第v+1行第u+1个元素的指针
	  if(d==0) continue;//d=0表示没有测到深度
	  
	  //point为相机坐标系
	  Eigen::Vector3d point;
	  point[2]=double(d)/depthScale;
	  point[0]=(u-cx)*point[2]/fx;
	  point[1]=(v-cy)*point[2]/fy;
	  
	  //通过T计算在世界坐标系下的坐标
	  Eigen::Vector3d pointWorld=T*point;
	  
	  //pcl点pointT  x,y,z,b,g,r(opencv下颜色通道的表示bgr)
	  PointT p;
	  p.x=pointWorld[0];
	  p.y=pointWorld[1];
	  p.z=pointWorld[2];
	  //不是很明白bgr怎么得到的
	  p.b=color.data[v*color.step+u*color.channels()];	//step 为图象像素行的实际宽度
	  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;
}

【疑问】（后面弄懂了再来更新）

1.相机位姿pose.txt如何求？

2.这里不是很懂，他这个格式怎么表达的？

//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读取原始

3.b,g,r如何得出来也不是很明白

//step为图象像素行的实际宽度
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];

安装pcl的时候出了一些问题，ubuntu14.04

1.安装依赖

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

2.下载源码

git clone https://github.com/PointCloudLibrary/pcl.git

3.编译源码

cd pcl
mkdir release
cd release
cmake -DCMAKE_BUILD_TYPE=None -DBUILD_GPU=ON -DBUILD_apps=ON -DBUILD_examples=ON ..
make
sudo make install

【坑】cmake的时候报错（已解决）

错误提示：CMake Error at CMakeLists.txt:2 (cmake_minimum_required):
                           CMake 3.5 or higher is required.  You are running version 2.8.12.2

可我查了一下，ubuntu14里cmake2.8就是最新版本啊~等我解决再来更新

*******************************************************************************************************************************************

解决方法：在不删除cmake2.8的情况下，更新cmake到3.9（如果想要其他版本，可以查找相应的安装包下载，最新的安装包可以在官网找到https://cmake.org/download/）

1.下载源码

wget https://cmake.org/files/v3.9/cmake-3.9.2.tar.gz

2.解压文件，进入文件夹路径

cd cmake-3.9.2
./configure

3.安装

sudo make install

安装完成后查看cmake的版本

cmake -version

到这里，我的cmake已经装完了，显示cmake version3.9.2

我看好多人会出现如下错误

CMake Error: Could not find CMAKE_ROOT !!!
CMake has most likely not been installed correctly.
Modules directory not found in
/usr/local/bin
段错误 (核心已转储)

解决方法（不知是否有效，没有试过~）

hash -r 

运行结果

至于为什么点云是单色的（别的博主都是彩色的）也不是很清楚，还没弄明白~没事继续往下学，说不定哪天就恍然大悟了！