拼接點雲思路

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.