图像处理技术之七：彩色图和深度图转点云

深度图像+rgb转化点云数据、点云数据打开、显示以及保存

环境：windows10、VS2013、opencv 2.49、openNi、PCL1.8
opencv 环境搭建参考
https://www.cnblogs.com/cuteshongshong/p/4057193.html
https://blog.csdn.net/u013105549/article/details/50493069
PCL1.8+openNi搭建参考
https://blog.csdn.net/u011197534/article/details/52960394
https://blog.csdn.net/wokaowokaowokao12345/article/details/47361369
将上面的opencv和pcl的配置保存到属性表中，以便下一次快速引用。

新建项目，选择解决方案配置选择Debug x64，属性管理器的Debug|x64中添加上面两个属性表

RGBDtoPC.cpp

 
#include "stdafx.h"
 
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
 
// 相机内参
const double camera_factor = 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;
// 主函数 
 
//#############深度图像+RGB图像 转 点云图像########################
int main(int argc, char** argv)
{
	// 读取./data/rgb.png和./data/depth.png，并转化为点云
 
	// 图像矩阵
	cv::Mat rgb, depth;
	// 使用cv::imread()来读取图像
	// API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
	rgb = cv::imread("color.png");
	cout << "read rgb"<<endl;
	// rgb 图像是8UC3的彩色图像
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	depth = cv::imread("depth.png");
	cout << "read depth" << endl;
	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	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_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];
		p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
		p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
 
		// 把p加入到点云中
		cloud->points.push_back(p);
		//cout << cloud->points.size() << endl;
		}
	// 设置并保存点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size = " << cloud->points.size() << endl;
	cloud->is_dense = false;
	try{
		//保存点云图
		pcl::io::savePCDFile("E:\\Visual Studio2013\\project\\RGBDtoPC\\data\\pcd.pcd", *cloud);
		
 
	}
	catch (pcl::IOException &e){
		cout << e.what()<< endl;
	}
	//显示点云图
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");//直接创造一个显示窗口
	viewer.showCloud(cloud);//再这个窗口显示点云
	while (!viewer.wasStopped())
	{
	}
 
	//pcl::io::savePCDFileASCII("E:\\Visual Studio2013\\projectpointcloud.pcd", *cloud);
	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
	return 0;
}


//###########点云图像  转换 深度图像 
首先定义相机的一些参数（可以根据自己的相机参数进行相机参数的相应修改）
const double camera_factor = 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;

深度图与点云数据之间的映射关系如下：

cv::Mat mDepth= cv::imread("C:\\Users\\XiRobot\\Desktop\\新兴铸管采图\\Right\\0_depth_09-19-16.png",-1);

PointCloud::Ptr cloud(new PointCloud);
// 遍历深度图
for (int m = 0; m < mDepth.rows; m++)
{
    for (int n = 0; n < mDepth.cols; n++)
    {
    // 获取深度图中(m,n)处的值
    ushort d = mDepth.ptr<ushort>(m)[n];
    // d 没有值则跳过此点
    if (d == 0)
    {
        continue;
    }
    PointT p;
    // 计算这个点的空间坐标
    p.z = double(d) / camera_factor;
    p.x = (n - camera_cx) * p.z / camera_fx;
    p.y = (m - camera_cy) * p.z / camera_fy;

    p.r = rand() % 255;
    p.g = rand() % 255;
    p.b = rand() % 255;
    cloud->points.push_back(p);
    }
}

//####深度图转换为点云数据就上面几行代码即可

//============================================================================
// 若需要将点云数据转换为深度图：

unsigned int iSum = 0;
for (int i = 0; i < mReslt.rows; i++)
{
    for (int j = 0; j < mReslt.cols; j++)
    {
    if (iSum < cloud3->points.size() && mReslt.ptr<ushort>(i)[j] != 1)//此处的判断是去掉像素为0的地方，像素为0的地方没有点云数据
    {
        mReslt.ptr<ushort>(i)[j] = cloud3->points[iSum].z*camera_factor;//255
        iSum++;
    }
    else
    {
        mReslt.ptr<ushort>(i)[j] = 0;
    }
}
}

运行后可能直接返回，提示pcl::io Exception
单步运行发现cv::imread()并没有读取到图片。原因如下
opencv有cvLoadImage()和cv::imread()读图片的方法
而后者的链接库版本不正确：（debug下对应的库为xxxd.lib，release的为xxx.lib） 
即链接器中的附加依赖项中同时添加带d和不带d的依赖项会出问题，如果用Debug调试则只添加后面带d的即可，将不带d的删除。

我添加了这些
opencv_calib3d249d.lib
opencv_contrib249d.lib
opencv_core249d.lib
opencv_features2d249d.lib
opencv_flann249d.lib
opencv_gpu249d.lib
opencv_highgui249d.lib
opencv_imgproc249d.lib
opencv_legacy249d.lib
opencv_ml249d.lib
opencv_nonfree249d.lib
opencv_objdetect249d.lib
opencv_photo249d.lib
opencv_stitching249d.lib
opencv_ts249d.lib
opencv_video249d.lib
opencv_videostab249d.lib
显示点云图参考：
https://blog.csdn.net/qq_16949707/article/details/53311083
 

//显示点云图
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");//直接创造一个显示窗口
	viewer.showCloud(cloud);//再这个窗口显示点云