#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry> 
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
#include <pcl/visualization/pcl_visualizer.h>

int main( int argc, char** argv )
{
    
    vector<cv::Mat> colorImgs, depthImgs;    //共有5张彩色图和5张深度图,使用容器依次存储这5张图。
    //Eigen::Isometry3d:欧式变换矩阵
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;// 相机位姿

    ifstream fin("./pose.txt");//ifstream:读操作（输入）的文件类
    if (!fin)
    {
    
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        return 1;
    }

    /* 初始化colorImgs，depthImgs，poses */
    for ( int i=0; i<5; i++ )
    {
    
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式
        //"color" 对应 %s； i+1对应 %d ； "jpg" 对应 %s
        //相当于cv::imread(./color1.jpg),以此类推。
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"jpg").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"jpg").str(), -1 )); // 使用-1读取原始图像

        double data[7] = {
    0};
        for ( auto& d:data ) //用auto& ,那么fin中的数据给了d，相应的也会给data
            fin>>d;//文件流类型的变量fin将pose.txt中的数据（3个平移数据，4个四元素）给了d数组

        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );//data[6]是实数,我们将实数写在前面，虚数写在后面
        //P_uv =K*T*P_w,K为内参数矩阵，T是由旋转R和平移t构成的矩阵矩阵（R，t为外参）
	    q=q.normalized();//归一化就是把T*P_w矩阵的最后一维化为1，便于与K相乘
        Eigen::Isometry3d T(q); //变换矩阵 初始化 旋转的这部分
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//变换矩阵 初始化 平移的这部分
        poses.push_back( T );//存储变换矩阵到位姿数组中
    }

    /* 计算点云并拼接 */
    // 相机内参
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0; //1000表示1m

    cout<<"正在将图像转换为点云..."<<endl;

    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud );
    for ( int i=0; i<5; i++ )
    {
    
        cout<<"转换图像中: "<<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
    
                //unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值，与下式等价
	            unsigned int d = depth.at<uchar>(v, u);// 深度值

                if (d == 0) continue; // 为0表示没有测量到
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;//书本P86页的Z
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T.inverse()*point;//T（3*3），point(3*1)

                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                pointCloud->points.push_back( p );
            }
    }

    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}