RGBD-SLAM学习3

继续跟着高博的博客学习，到了第三篇：
http://www.cnblogs.com/gaoxiang12/p/4659805.html

第三篇其实是分了两个部分：一个就是将产生点云这个功能封装成一个库；另外一个就是将图像匹配得位姿变换。

将产生点云功能封装成一个库：
首先说一下，一个程序编译链接之后，并不一定都生成可执行程序，有的是生成了库，以备其他程序调用。所以这两个有点并行的意味。首先，一个库要有头文件和库文件：
头文件slamBase.h，放在include文件夹中：

//
// Created by robin on 18-1-29.
//

//# pragma once

// 各种头文件
// C++标准库
#include <fstream>
#include <vector>
using namespace std;

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

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

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

// 相机内参结构
struct CAMERA_INTRINSIC_PARAMETERS
{
    double cx, cy, fx, fy, scale;
};

// 函数接口
// image2PonitCloud 将rgb图转换为点云
PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );

// point2dTo3d 将单个点从图像坐标转换为空间坐标
// input: 3维点Point3f (u,v,d)，注意下这里比较奇葩，这个三维向量并不是坐标，没有什么实际意义，仅仅是一个数列，分别存储了像素坐标uv和深度数据d。用于给函数传递参数。
cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

内容很简单，就是定义了一个存储相机内参的结构体，定义了两个功能函数。

再看src中的源文件slamBase.cpp：

//
// Created by robin on 18-1-29.
//

#include "slamBase.h"

PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    PointCloud::Ptr cloud ( new PointCloud );

    for (int m = 0; m < depth.rows; m++)
        for (int n=0; n < depth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
                continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / camera.scale;
            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];
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];

            // 把p加入到点云中
            cloud->points.push_back( p );
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cloud->is_dense = false;

    return cloud;
}

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    cv::Point3f p; // 3D 点
    p.z = double( point.z ) / camera.scale;
    p.x = ( point.x - camera.cx) * p.z / camera.fx;
    p.y = ( point.y - camera.cy) * p.z / camera.fy;
    return p;
}

更明了，include进来头文件后，写两个功能函数的定义就好了。

主要说的是CMakeLists.txt的书写，按照动改哪个文件夹的文件就改动哪个文件夹中的CMakeLists.txt的原则，我们对src中的文件做了改动(写了个slamBase.cpp文件嘛～)，所以要改动的也是src中的CMakeLists.txt。
首先，最直观的就是我们要生成一个库，那add_library和target_link_libraries不就好了么，生成库，并将库链接到依赖库上，所以在原来的CMakeLists.txt基础上增