1.定义图片变量和相机位姿。用vector<cv::Mat>和vector<Eigen::Isometry3d>就可以了。
2.读取
（1）先设地址，位姿地址就一个。用ifstream fin("./pose.txt")直接读取就可以了。而图片地址有多个。就需要在for循环里，先boost::format fmt("../%s/%d.%s")，把图像文件格式给统一一下。那输入的时候是fmt%什么就可以了。就像之前输入文档一样。前面%s,后面%跟实际值就可以了。如果是字符串，用双引号。
然后读取图片的时候用push_back函数里面再套一个cv::imread的函数。cv::imread是用来读取图片的。之前就定义过image=cv::imread(argv[1]),cv::imread读的是参数，所以fmt还要再.str()一下。
就是colorImage.push_back(cv::imread((fmt%"color"%(i+1)%"png").str()))就可以了。
如果读取原始图像，cv::imread后面还要再加一个参数-1.
（2）位姿数据里面定义的有7个量，前面3个是位移，后面四个是四元数，最后一个是实部。
读取位姿的思路是这样的。先定义一个7个变量的数组并初始化为0。然后定义一引用，一个for循环，让for循环遍历data中的每一元素d,并给每个元素赋位姿里的值。fin>>d.
double data[7]={0};
然后用这7个值构成一个位姿T.
放在i循环下，说明i=1的时候对应一个位姿，每个I对应的位姿并不相同啊。
3.定义内参和尺度，方便之后计算。
4.定义ＲＧＢ值PointT和点云pointcloud.
5.由图片，可以知道像素坐标。下面就是要由像素坐标计算出像素在相机坐标系下的坐标。公式简单。
z=d/尺度因子。
d是每个像素对应的深度值，可以通过depth.ptr<unsigned short>读取出来。
x=(u-cx)*z/fx;
y=(u-cy)*z/fy.得到。
然后通过相机位姿算出在世界坐标系下的坐标。
定义一个pcl点p.这个p有六个值，x,y,z，b,g,r。x,y,z就是世界坐标系下的坐标，b,g,r就是颜色值。这6个值构成了点p.点云的points.push_back()把一个一个点P放进去，就构成了点云。
6.拼接点云
拼接点云特别简单。用一个函数pcl::io::savePCDFileBinary()就可以了。函数参数是要存的点云名字，和要拼接的点云名称。注意，点云是指针形式。
看点云用pcl_viewer.