#include <QtCore/QCoreApplication>
#include <iostream>
#include <string>
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;
// 相機內參
const double camera_factor = 700;
const double camera_cx = 256;
const double camera_cy = 212;
const double camera_fx = 363.0;
const double camera_fy = 363;
int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
// 讀取./data/rgb.png和./data/depth.png,並轉化爲點雲
// 圖像矩陣
cv::Mat rgb, depth;
// 使用cv::imread()來讀取圖像
rgb = cv::imread("F:/depth.png");
// rgb 圖像是8UC3的彩色圖像
// depth 是16UC1的單通道圖像,注意flags設置-1,表示讀取原始數據不做任何修改
depth = cv::imread("F:/depth.png", -1);
// 點雲變量
// 使用智能指針,創建一個空點雲。這種指針用完會自動釋放。
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);
}
// 設置並保存點雲
cloud->height = 1;
cloud->width = cloud->points.size();
cout << "point cloud size = " << cloud->points.size() << endl;
cloud->is_dense = false;
pcl::io::savePCDFile("F:/222.pcd", *cloud);
// 清除數據並退出
cloud->points.clear();
cout << "Point cloud saved." << endl;
return a.exec();
}