視覺slam十四講ch5 joinMap.cpp 代碼註釋（筆記版）
阿新 • • 發佈：2018-02-03
iterator 實驗 article ons 如果 false 結果 插入 智能指針類
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 * */
視覺slam十四講ch5 joinMap.cpp 代碼註釋（筆記版）