#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();
}