圖像的點(diǎn)云拼接-原理講解與代碼實(shí)現(xiàn)
理解好圖像的點(diǎn)云拼接，需要從相機(jī)的模型說(shuō)起。理解相機(jī)的成像原理之后，便可更為深刻的理解圖像的點(diǎn)云如何拼接在一起。
首先說(shuō)下相機(jī)的概念與原理。
相機(jī)概念與原理
相機(jī)的作用：將三維世界中的坐標(biāo)點(diǎn)(單位為米)映射到二維圖像平面(單位為像素)。
通常我們見(jiàn)到的相機(jī)都是針孔相機(jī)，但是不是簡(jiǎn)單的 針孔，還有透鏡的畸變存在，所以在做圖像處理時(shí)要進(jìn)行畸變校正。
由于畸變的存在，我們?cè)谑褂孟鄼C(jī)之前都要進(jìn)行相機(jī)標(biāo)定。
目的就是求出內(nèi)參對(duì)于簡(jiǎn)單的應(yīng)用求出徑向畸變和切向畸變就夠了。
所謂的外參就是相機(jī)的位姿。
有了外參、內(nèi)參、圖像、深度信息 便可以把圖像中的點(diǎn)，轉(zhuǎn)到世界坐標(biāo)系下，并帶有RGB的顏色。就形成了所謂的點(diǎn)云。
相關(guān)的公式也很好理解 ，就是通過(guò)相似三角形的原理。
注意其中fx 、fy、 cx 、cy也就是相機(jī)的內(nèi)參了。在相機(jī)出廠時(shí)會(huì)給出。
理解了相機(jī)的概念與原理，來(lái)編輯代碼實(shí)現(xiàn)下。
Code實(shí)現(xiàn)
全部代碼
先放上全部代碼，再進(jìn)行分段講解
using namespace std;int main( int argc, char** argv ){vector<cv::Mat> colorImgs,depthImgs;//彩色圖和深度圖vector<Eigen::Isometry3d> poses;//相機(jī)位姿ifstream fin("../pose.txt");//文件讀入 相機(jī)位置 文件if(!fin)//相機(jī)位姿讀入失敗{cerr<<"請(qǐng)?jiān)谟衟ose.txt的目錄下運(yùn)行此程序"<<endl;return 1;}for(int i=0;i<5;i++){boost::format fmt("../%s/%d.%s");//圖像文件格式colorImgs.push_back(cv::imread((fmt%"color"%(i+1)%"png").str())); // 讀取彩色圖像depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用 -1 讀取原始圖像//讀取相機(jī)位姿double data[7]={0};for(auto& d:data)fin>>d;Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );Eigen::Isometry3d T(q);T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));poses.push_back( T );}//相機(jī)內(nèi)參double cx=325.5;double cy=253.5;double fx=518.0;double fy=519.0;double depthScale = 1000.0;cout<<"正在將圖像轉(zhuǎn)換為點(diǎn)云..."<<endl;//定義點(diǎn)云使用的格式 用XYZRGBtypedef pcl::PointXYZRGB PointT;typedef pcl::PointCloud<PointT> PointCloud;//新建一個(gè)點(diǎn)云PointCloud::Ptr pointCloud( new PointCloud );for(int i=0;i<5;i++){cout<<"轉(zhuǎn)換圖像中："<<i+1<<endl;cv::Mat color = colorImgs[i];//獲得 要處理的彩色圖像cv::Mat depth = depthImgs[i];//獲得 要處理的深度圖像Eigen::Isometry3d T = poses[i];//獲得 對(duì)應(yīng)的posefor(int v=0 ; v<color.rows;v++)//處理每個(gè)像素{for(int u=0 ; u<color.cols;u++){unsigned int d = depth.ptr<unsigned short>(v)[u];//深度值if(d==0) continue;//為0 表示沒(méi)有測(cè)量到Eigen::Vector3d point;//相機(jī)坐標(biāo)系下的點(diǎn)//計(jì)算相機(jī)坐標(biāo)系下的點(diǎn)的坐標(biāo)point[2] = double(d)/depthScale;point[0] = (u-cx)*point[2]/fx;point[1] = (v-cy)*point[2]/fy;Eigen::Vector3d pointWorld = T*point;//轉(zhuǎn)換到世界坐標(biāo)系下PointT p;//聲明點(diǎn)云p.x = pointWorld[0];//賦值點(diǎn)云位置p.y = pointWorld[1];p.z = pointWorld[2];p.b = color.data[ v*color.step+u*color.channels() ];//賦值點(diǎn)云對(duì)應(yīng)RGB顏色p.g = color.data[ v*color.step+u*color.channels()+1 ];p.r = color.data[ v*color.step+u*color.channels()+2 ];pointCloud->points.push_back( p );}}}pointCloud->is_dense = false;cout<<"點(diǎn)云共有"<<pointCloud->size()<<"個(gè)點(diǎn)."<<endl;pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//保存點(diǎn)云文件return 0;}
代碼講解
vector<cv::Mat> colorImgs,depthImgs;//彩色圖和深度圖vector<Eigen::Isometry3d> poses;//相機(jī)位姿ifstream fin("../pose.txt");//文件讀入 相機(jī)位置 文件if(!fin)//相機(jī)位姿讀入失敗{cerr<<"請(qǐng)?jiān)谟衟ose.txt的目錄下運(yùn)行此程序"<<endl;return 1;}for(int i=0;i<5;i++){boost::format fmt("../%s/%d.%s");//圖像文件格式colorImgs.push_back(cv::imread((fmt%"color"%(i+1)%"png").str())); // 讀取彩色圖像depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用 -1 讀取原始圖像//讀取相機(jī)位姿double data[7]={0};for(auto& d:data)fin>>d;Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );Eigen::Isometry3d T(q);T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));poses.push_back( T );}
讀取對(duì)應(yīng)路徑下的彩色圖像、深度圖像、相機(jī)的位姿。
上面說(shuō)了，有了這三個(gè)量再加上相機(jī)內(nèi)參，就可以得到點(diǎn)云。
//相機(jī)內(nèi)參double cx=325.5;double cy=253.5;double fx=518.0;double fy=519.0;double depthScale = 1000.0;
傳說(shuō)中的相機(jī)內(nèi)參
//定義點(diǎn)云使用的格式 用XYZRGBtypedef pcl::PointXYZRGB PointT;typedef pcl::PointCloud<PointT> PointCloud;//新建一個(gè)點(diǎn)云PointCloud::Ptr pointCloud( new PointCloud );
聲明pcl的格式創(chuàng)建一個(gè)點(diǎn)云，然后開(kāi)始for循環(huán)處理每一張圖片和對(duì)應(yīng)的深度圖片與相機(jī)位姿。
cv::Mat color = colorImgs[i];//獲得 要處理的彩色圖像cv::Mat depth = depthImgs[i];//獲得 要處理的深度圖像Eigen::Isometry3d T = poses[i];//獲得 對(duì)應(yīng)的pose
得到要處理的彩色圖像深度圖像對(duì)應(yīng)的pose，然后for循環(huán)處理每一個(gè)像素。
unsigned int d = depth.ptr<unsigned short>(v)[u];//深度值得到深度信息
Eigen::Vector3d point;//相機(jī)坐標(biāo)系下的點(diǎn)//計(jì)算相機(jī)坐標(biāo)系下的點(diǎn)的坐標(biāo)point[2] = double(d)/depthScale;point[0] = (u-cx)*point[2]/fx;point[1] = (v-cy)*point[2]/fy;
通過(guò)：
這個(gè)公式得到相機(jī)坐標(biāo)系下的 x，y，z
Eigen::Vector3d pointWorld = T*point;//轉(zhuǎn)換到世界坐標(biāo)系下相機(jī)坐標(biāo)系下的點(diǎn)通過(guò)坐標(biāo)變換轉(zhuǎn)到世界坐標(biāo)系下。
PointT p;//聲明點(diǎn)云p.x = pointWorld[0];//賦值點(diǎn)云位置p.y = pointWorld[1];p.z = pointWorld[2];p.b = color.data[ v*color.step+u*color.channels() ];//賦值點(diǎn)云對(duì)應(yīng)RGB顏色p.g = color.data[ v*color.step+u*color.channels()+1 ];p.r = color.data[ v*color.step+u*color.channels()+2 ];
賦值點(diǎn)云的坐標(biāo)與顏色。
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//保存點(diǎn)云文件保存成點(diǎn)云文件。
—版權(quán)聲明—
來(lái)源：古月居
僅用于學(xué)術(shù)分享，版權(quán)屬于原作者。
若有侵權(quán)，請(qǐng)聯(lián)系微信號(hào):yiyang-sy 刪除或修改！
—THE END—