//?C++?標(biāo)準(zhǔn)庫
#include?
#include?
using?namespace?std;

//?OpenCV?庫
#include?
#include?

//?PCL?庫
#include?
#include?
#include?

//?定義點(diǎn)云類型
typedef?pcl::PointXYZRGBA?PointT;
typedef?pcl::PointCloud?PointCloud;?

//?相機(jī)內(nèi)參
const?double?camera_factor?=?1000;
const?double?camera_cx?=?979.674;
const?double?camera_cy?=?535.383;
const?double?camera_fx?=?1043.02;
const?double?camera_fy?=?1047.78;

//?主函數(shù)?
int?main（?int?argc?char**?argv?）
{
????//?讀取./data/rgb.png和./data/depth.png，并轉(zhuǎn)化為點(diǎn)云

????//?圖像矩陣
????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（?“0_Color.png“?）;
????//?rgb?圖像是8UC3的彩色圖像
????//?depth?是16UC1的單通道圖像，注意flags設(shè)置-1表示讀取原始數(shù)據(jù)不做任何修改
????depth?=?cv::imread（?“0_Depth.png“?-1?）;

????//?點(diǎn)云變量
????//?使用智能指針，創(chuàng)建一個空點(diǎn)云。這種指針用完會自動釋放。
????PointCloud::Ptr?cloud?（?new?PointCloud?）;
????//?遍歷深度圖
????for?（int?m?=?0;?m?????????for?（int?n=0;?n?????????{
????????????//?獲取深度圖中（mn）處的值
????????????ushort?d?=?depth.ptr（m）[n];
????????????//?d?可能沒有值，若如此，跳過此點(diǎn)
????????????//?if?（d?==?0）
????????????//?????continue;
????????????//?d?存在值，則向點(diǎn)云增加一個點(diǎn)
????????????PointT?p;

????????????//?計算這個點(diǎn)的空間坐標(biāo)
????????????p.z?=?double（d）?/?camera_factor;
?????????????//?if（p.z>1）
?????????????//????continue;
????????????p.x?=?（n?-?camera_cx）?*?p.z?/?camera_fx;
?????????????//?if（p.x<-0.15||p.x>0.15）
?????????????//????continue;
????????????p.y?=?-（m?-?camera_cy）?*?p.z?/?camera_fy;
????????????
????????????//?從rgb圖像中獲取它的顏色
????????????//?rgb是三通道的BGR格式圖，所以按下面的順序獲取顏色
????????????p.b?=?rgb.ptr（m）[n*3];
????????????p.g?=?rgb.ptr（m）[n*3+1];
????????????p.r?=?rgb.ptr（m）[n*3+2];

????????????//?把p加入到點(diǎn)云中
????????????cloud->points.push_back（?p?）;
????????}
????//?設(shè)置并保存點(diǎn)云
????cloud->height?=?1;
????cloud->width?=?cloud->points.size（）;
????cout<<“point?cloud?size?=?“<points.size（）<????cloud->is_dense?=?false;
????pcl::io::savePCDFile（?“./0_image.pcd“?*cloud?）;
????pcl::io::savePLYFile（?“./0_image.ply“?*cloud?）;
????//?清除數(shù)據(jù)并退出
????cloud->points.clear（）;
????cout<<“Point?cloud?saved.“<????return?0;
}