1 #include <iostream>
  2 #include <fstream>
  3 using namespace std;
  4 #include <opencv2/core/core.hpp>
  5 #include <opencv2/highgui/highgui.hpp>
  6 #include <Eigen/Geometry> 
  7 #include <boost/format.hpp>  // for formating strings
  8 #include <pcl/point_types.h> 
  9 #include <pcl/io/pcd_io.h> 
 10 #include <pcl/visualization/pcl_visualizer.h>
 11 
 12 int main( int argc, char** argv )
 13 {
 14     /*彩色圖和灰度圖各5張，所以用容器來儲存*/
 15     vector<cv::Mat> colorImgs, depthImgs;    // 彩色圖和深度圖
 16 
 17     /*vector有兩個引數，後面的引數一般是預設的，這裡用適合Eigen庫的對齊方式來初始化容器，總共有5張圖片 所以對應著5個位姿矩陣*/
 18     vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;   // 相機位姿
 19     
 20     ifstream fin("./pose.txt");
 21     if (fin.bad())//如果沒有開啟 那麼提示錯誤！
 22     {
 23         cerr<<"請在有pose.txt的目錄下執行此程式"<<endl;
 24         return 1;
 25     }
 26 
 27     /*迴圈讀取影象*/
 28     for ( int i=0; i<5; i++ )
 29     {
 30         /*用boost中的format格式類，來迴圈讀取圖片，否則單張讀取圖片就會有問題
 31          * 當在命令列中執行的時候這裡必須要為../  在當前ide中執行的時候要修改為./ */
 32         boost::format fmt( "./%s/%d.%s" ); //影象檔案格式   "../%s/%d.%s"   ../ 表示可執行檔案在build中,影象在上一個目錄,所以用../
 33         /*這裡的%對應./ color對應%s  下面的符號就是與上面一致對應的 */
 34         colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
 35         depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1讀取原始影象
 36 
 37         /*基於範圍的for迴圈，表示從data陣列的第一項開始 迴圈遍歷  auto表示自動根據後面的元素 獲得符合要求的型別*/
 38         double data[7] = {0};
 39         for ( auto& d:data )//auto自動型別轉換
 40             fin>>d;//檔案流型別的變數fin將pose.txt中的資料給了d陣列
 41         Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元數 data[6]是實數 但是coeffis輸出的是先虛數後實數
 42         Eigen::Isometry3d T(q);                                     //變換矩陣初始化旋轉部分，
 43         T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//變換矩陣初始化平移向量部分
 44         poses.push_back( T );   //儲存變換矩陣到位姿陣列中
 45     }
 46     
 47     // 計算點雲並拼接
 48     // 相機內參 
 49     double cx = 325.5;
 50     double cy = 253.5;
 51     double fx = 518.0;
 52     double fy = 519.0;
 53     double depthScale = 1000.0;//
 54     
 55     cout<<"正在將影象轉換為點雲..."<<endl;
 56     
 57     // 定義點雲使用的格式：這裡用的是XYZRGB
 58     typedef pcl::PointXYZRGB PointT; 
 59     typedef pcl::PointCloud<PointT> PointCloud;
 60     
 61     // 新建一個點雲//PointCoud::Ptr是一個智慧指標類 通過建構函式初始化指標指向的申請的空間
 62     /*Ptr是一個智慧指標，返回一個PointCloud<PointT> 其中PointT是pcl::PointXYZRGB型別。它過載了->  返回了指向PointCloud<PointT>的指標
 63      *Ptr是下面型別 boost::shared_ptr<PointCloud<PointT> > */
 64     /*pointCloud 是一個智慧指標型別的物件 具體可以參考http://blog.csdn.net/worldwindjp/article/details/18843087*/
 65     PointCloud::Ptr pointCloud( new PointCloud );
 66 //    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );
 67 
 68     /*將5張圖片 畫素座標轉換到相機座標 之後轉換到世界座標儲存到點雲格式的變數中 for迴圈之後用pcl的相關函式將點雲轉換到pcl能夠顯示的格式*/
 69     for ( int i=0; i<5; i++ )//轉換5張影象
 70     {
 71         cout<<"轉換影象中: "<<i+1<<endl; 
 72         cv::Mat color = colorImgs[i]; 
 73         cv::Mat depth = depthImgs[i];
 74         Eigen::Isometry3d T = poses[i];
 75 
 76         /*           插入部分
 77          * //color.at<cv::Vec3b>(471,537)[0] = 12;//修改影象上的對應畫素位置的值
 78         //color.ptr<cv::Vec3b>(471)[537][0] = 12;//與上面的效果一樣
 79 
 80         //測試畫素的輸出效果,這裡無法通過cout<<color.at<cv::Vec3b>(471,537)[0] 這種方式來輸出第一個通道的值,因為每個通道的畫素佔了8位而unsigned char
 81         // 表示ascii碼 所以輸出的時候不是正確的數字，可以通過下面的方式強制轉化為int型別（或者用自帶的型別轉換方式進行顯示轉換），就可以看到內部的值了
 82         //需要注意的一點是 cout頁無法輸出char型別的變數的地址，也是需要強制轉換成void *型別的指標才能正常輸出char型別變數的地址資訊。
 83         if(colorImgs[i].channels() == 3) {
 84              std::cout << "測試1結果 " << color.ptr<cv::Vec3b>(471)[537] << "正確的結果:  "
 85                        << (char) color.at<cv::Vec3b>(471, 537)[0] << std::endl;
 86              std::cout << depth.ptr<unsigned short>(471)[537] << std::endl;
 87              std::cout << colorImgs[i].at<cv::Vec3b>(471, 537) << std::endl;
 88         }
 89                      插入部分結束
 90          */
 91 
 92         /*對影象畫素進行座標轉換，將影象的座標通過內參矩陣K轉換為相機座標系下的座標，之後通過外參矩陣T 轉化為世界座標系下的座標*/
 93         for ( int v=0; v<color.rows; v++ )
 94             for ( int u=0; u<color.cols; u++ )
 95             {
 96                 /*通過用Mat中的ptr模板函式 返回一個unsigned short型別的指標。v表示行 根據內部計算返回data頭指標 + 偏移量來計算v行的頭指標
 97                  * 影象為單通道的   depth.ptr<unsigned short> ( v ) 來獲取行指標*/
 98                 unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值16位儲存 一般color影象畫素中每一個通道是8位
 99 
100                  /*             單通道遍歷影象的方式總結:
101                  *  注意深度影象的畫素佔16位 與普通圖片每個通道的畫素為8位不同
102                  * 1、同樣是用上面的雙層for迴圈，遍歷影象 用at方式
103                  * for ( int v=0; v<color.rows; v++ )
104                  *      for ( int u=0; u<color.cols; u++ )
105                  *          unsigned int d = depth.at<unsigned short >(v,u);
106                  *
107                  *
108                  * 2、使用迭代器進行影象的遍歷
109                  * 不是基於for迴圈了
110                  * cv::MatIterator_<unsigned short > begin,end;
111                  * for( begin =depth.begin<unsigned short >(), end = depth.end<unsigned short >(); begin != end; ){}
112                  *
113                  * 3、使用指標的方式 如本實驗的結果
114                  * */
115 
116                 //迭代器的引數是通道數，因為深度圖是單通道的，每個畫素的值是unsigned short，所以引數是unsigned short
117                 //begin代表畫素的開始地方
118                 if ( d==0 ) continue;                // 為0表示沒有測量到 然後繼續進行for迴圈那麼跳過這個畫素繼續執行 在後面形成點雲時需要設定is_dense為假
119                 Eigen::Vector3d point;
120                 point[2] = double(d)/depthScale;    //對實際尺度的一個縮放
121                 point[0] = (u-cx)*point[2]/fx;      //根據書上5.5式子---86頁
122                 point[1] = (v-cy)*point[2]/fy; 
123                 Eigen::Vector3d pointWorld = T*point;   //將相機座標系轉換為世界座標系
124                 
125                 PointT p ;
126                 p.x = pointWorld[0];    //將世界座標系下的座標用pcl專門的點雲格式儲存起來
127                 p.y = pointWorld[1];
128                 p.z = pointWorld[2];
129 
130                 /*  color.step 雖然是一個類，但是它內部有一個轉換操作符 operator size_t() const;
131                  * 此時的color.size編譯器就會把它當做size_t型別的變數,這個值的大小是1920 這個是隨著影象的讀入MAT類中會有自動轉換然後儲存的buf[]中 */
132                 p.b = color.data[ v*color.step+u*color.channels() ];
133                 p.g = color.data[ v*color.step+u*color.channels()+1 ];
134                 p.r = color.data[ v*color.step+u*color.channels()+2 ];
135 
136                 /*  -> 是智慧指標過載的 然後返回的型別就是輸入的型別 可以看上面Ptr的解釋說明 */
137                 pointCloud->points.push_back( p );//儲存格式好的點
138             }
139     }
140     std::cout<<"點雲的列和行為 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
141     //這裡有可能深度圖中某些畫素沒有深度資訊，那麼就是包含無效的畫素，所以先置為假，但是如果設定成true的話 也沒有看出來有什麼不一樣的地方
142     pointCloud->is_dense = false;
143     std::cout<<"點雲的列和行為 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
144 
145     cout<<"點雲共有"<<pointCloud->size()<<"個點."<<endl;
146     pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//獲取pointCloud指向的物件 這個就當做獲取普通指標指向的物件來理解，這個物件是在定義的時候new出來的一段記憶體空間。
147     return 0;
148 
149 }
150 /*                                            備註： 3通道的影象的遍歷方式總結
151  * 對於單通道來說 每個畫素佔8位 3通道則是每個矩陣元素是一個Vec3b 即一個三維的向量 向量內部元素為8位數的unsigned char型別
152  * 1、使用at遍歷影象
153  * for(v)row
154  *  for(u)col
155  *      image.at<Vec3b>（v,u）[0] 表示第一個通道的畫素的值
156  *      image.at<Vec3b>(v,u)[1]
157  *      image.at<Vec3b>(v,u)[2]
158  * 2、使用迭代器方式 (實際上就是一個指標指向了 cv::Mat矩陣元素)
159  * cv::MatIterator_<Vec3b>begin,end;
160  * for( begin = image.begin<Vec3b>(), end = image.end<Vec3b>() ; begin != end;  )
161  *      (*begin)[0] = ...
162  *      (*begin)[1] = ...
163  *      (*begin)[2] = ...
164  *
165  * 3、用指標的方式操作
166  * for(v)
167  *  for(u)
168  *      image.ptr<Vec3b>(v)[u][0] 表示第一個通道
169  *      image.ptr<Vec3b>(v)[u][0] 表示第二通道
170  *              .
171  *              .
172  *              .
173  * */