ICP+点云拼接实践

写程序时觉得自己的逻辑和思路比较混乱，而且仅仅是在写比较小的程序、无需构架方面的考虑时。比如，试图修改14讲的ICP算法，使之结合拼接点云实验时，需要将前面匹配的keypoints坐标、颜色、深度保存，并在拼接点云时作为参数传入，半天不知道该在哪里加，总感觉原本的写法用起来不太会。遂从提取特征点开始，重写之。
第一步，先搞清楚每个部分如何实现吧，十分具体的框图如下

接下来，要往里面添加存储颜色、坐标的代码。匹配点最后筛选时入栈了，那对于之前检测到的特征点，检测时我们也一起提取出颜色、坐标、深度，在符合要求匹配点入栈时，一起使之入栈，是否可行？
在点云拼接实验中
colorImgs, depthImgs的类型是vector

vector<cv::Mat> colorImgs, depthImgs;
colorImgs.push_back(cv::imread(picture[i].str()));
depthImgs.push_back(cv::imread(picture[i].str(),-1));//使用-1读取原始图像？啥意思
cv::Mat color = colorImgs[i];
cv::Mat dept