2、RGB-D 从图像到点云 detectFeatures

（在做高博的“一起做RGB-D_SLAM”的一些问题，作为自己笔记总结,以督促自己完成并理解)
一起做－－从图像到点云
接着上面的在qt下新建ｓｌａｍ项目来的。
1、在src/CMakeLists.txt下添加：

#############################generatePointCloud
# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )

# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )

# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

ADD_EXECUTABLE( generate_pointcloud gpointcloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS}
    ${PCL_LIBRARIES} )

在qt里面，右上角，选File -> new file or project -> c++ ->c++ source file 输入name:” generatePointCloud.cpp “　　代码与高博的一起做一样，照着打的

#include <iostream>
#include <string>

using namespace std;

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

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCLoud;


// 相机内参
const double camera_factor = 1000;                        //通常1000的值代表1米,深度图里每个像素点的读数除以1000，就是它离你的真实距离了
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;

int main (int argc, char** argv)
{
    cv::Mat rgb, depth;                         //读取./data/rgb.png和./data/depth.png，并转化为点云
    rgb = cv::imread( "./data/rgb.png" );
    depth = cv::imread("./data/depth.png",-1);       　//注意flags设置-1,表示读取原始数据不做任何修改

    PointCLoud::Ptr cloud (new PointCLoud);      // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    for (int m = 0 ;m <depth.rows; m++)                  //我们按照“先列后行”的顺序，遍历了整张深度图
        for (int n = 0;n <depth.cols; n++)
        {
            ushort d = depth.ptr<ushort>(m)[n];                //深度图第m行，第n行的数据可以使用depth.ptr<ushort>(m) [n]来获取
            //其中，cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后，这个指针指向的数据就是我们需要读取的数据
            if (d == 0)                                        // d 可能没有值，若如此，跳过此点
                continue;
            PointT p;                                         // d 存在值，则向点云增加一个点

            p.z = double(d) / camera_factor;  先求出空间点
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;


            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(m)[n*3];               //通过depth.ptr<ushort>能读出i行j列的深度值
            if(d==0)                                     //如果是hole，则直接跳过
                continue;
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];

            cloud->points.push_back( p );                  //将p储存到cloud中

        }
    cloud->height = 1;                                     //设点云数据
    cloud->width = cloud->points.size();
    cout<<"point cloud size = "<<cloud->points.size()<<endl;
    cloud->is_dense = false;
    pcl::io::savePCDFile("./pointclod.pcd", *cloud);　//把整个点云存储为 ./data/pointcloud.pcd 文件
  // 清除数据并退出
    cloud->points.clear();
    cout<<"Point cloud saved."<<endl;
    return 0;
}

将data里的两张图片放到ｓｌａｍ项目目录下，然后编译
但注意不能在qt里直接运行，会出现这个错误：

打开终端输入，在ｓｌａｍ项目目录下

$　bin/generate_pointcloud

即可在data目录下生成点云文件。现在，你肯定希望查看一下新生成的点云了。如果已经安装了pcl，就可以通过：

$　pcl_viewer pointcloud.pcd

来查看新生成的点云：