主要用的g2o的方法。本身定義了一個新的一元邊。邊的偏差項是測量值和由估計得來的x,y對應的灰度值之間的偏差。導數爲灰度對像素座標的導數乘以像素座標對yi*李代數的導數的負數。灰度對於像素座標的導數矩陣爲1*2的矩陣。好比像素座標爲u,v.第一個數就是灰度關於u的導數,爲u+1,v對應的灰度值和u-1,v對應的灰度值之差再除以2.v同理。
而像素座標對yi*李代數的導數爲2*6的雅克比矩陣。這個導數能夠轉化爲兩個導數的乘積,一個是像素座標對空間點的導數,一個是空間點對yi*李代數的導數。
像素座標和空間點有這樣的關係
u=x*fx/z+cx
v=y*fy/z+cy.
u對x,y,z進行求導分別爲fx/z,0,-x*fx/(z*z)
v對x,y,z進行求導分別爲0,fy/z,-y*fy/(z*z).
而空間點x,y,z對yi*李代數的導數爲[I,-空間點的反對稱矩陣】
這樣二者相乘就能夠了。
由於求解偏差項的時候和求解灰度對於像素座標的導數的時候都要用到估計值的灰度值,因此要有一個灰度值函數,這個灰度值函數是對圖像進行雙線性插值獲得的。所謂的雙線性插值就是
a+bx+cy+dxy.
這裏的a能夠爲(1-xx)*(1-yy),b=xx*(1-yy),c=yy(1-xx),d=xx*yy.其中xx=x-deltax,y=y=deltay.
g2o的頂點就是估計值,能夠用_vertices[0]賦值,賦值以後就用這個值計算估計值,有估計值就能夠計算偏差等。進而迭代找到最優的估計值。
如今來看整個程序是怎麼實現的。
1.讀圖和設置一些須要的變量。
定義圖像數據路徑,定義associate.txt文件路徑,定義圖像矩陣color,depth,gray,以後fin能夠把aaociate裏的每一行來讀取相對應的彩色圖和深度圖。
定義fx,fy,cx,cy,depth_scale.fx,fy,cx,cy都是相機內參矩陣的組成部分,depth_scale爲深度圖縮放因子。有前四項能夠獲得K,depth_scale以後待用。
定義Tcw類型和初值,Eigen::Isometry3d::Identity().
定義prev_color,以後就把第一張圖片設置成參考圖。
2.作一個for循環,在這個循環裏,不斷地讀取associate.txt裏對應的數據集下的彩色圖和深度圖,而後賦值給color和depth.但只有第一張圖片爲prev_color,也只對第一張圖片提取關鍵點。提取關鍵點以後,對關鍵點進行篩選,閾值是20.篩選以後,獲得這些關鍵點的空間點座標和灰度值,組成一個結構體,放到測量值裏。
其中獲得空間點要通過
z=d/depth_scale,
x=(u-cx)*z/fx
y=(u-cy)*z/fy
其中d是深度值,須要在深度圖上讀取,記住座標是先y後x.並且是(y)[int x]
獲得灰度值,首先須要獲得灰度圖,灰度圖是由彩色圖獲得的。用cv::cvtColor函數。
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
而後從灰度圖裏讀取像素座標對應的灰度值,也是先v後u的,並且是(v)[u],其實應該都是u,v,可是這裏都表示成了x,y.
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
3.構造稀疏直接法位姿估計函數poseEstimationDirect函數
這裏的變量只須要測量值measurements,下一張圖片的灰度圖&gray,相機內參矩陣K,和要求的Tcw就能夠了。
這個函數沒有什麼特別的,無非仍是先設置估計的值的類型,求解器,求解的方法,這裏是列文伯格。圖模型optimizer,圖模型求解方法列文伯格,圖模型設置調試輸出.setVerbose(true)。
主要來看咱們本身造的邊。
這個邊類裏面有幾個變量,分別是空間點座標,fx,fy,cx,cy,*image.空間點座標以後放的是測量值的空間點座標,也就是第一張圖像的關鍵點對應的空間點座標。*image放的是下一張圖像的灰度圖。
計算偏差的時候會把估計值複製給v.而後v是位姿,把空間點映射成第二張圖片的空間點,而後通過內參矩陣,映射以後的空間點轉換爲第二張圖片上的像素座標,對這些像素座標進行篩選,閾值爲4.若是不在範圍內,偏差值設爲0,level設爲1.若是在範圍內,偏差爲這些偏差值對應的灰度值和測量值的灰度值相減。基於灰度不變假設。
而在邊類的計算雅克比矩陣的函數linearizeOplus函數裏,若是level=1,就是以前計算獲得的像素座標不在範圍內,最終的雅各比矩陣設爲0.最終雅克比矩陣形式爲1*6的,爲
_jacobianOplusXi=Eigen::Matrix<double,1,6>::Zero().
一樣,這裏吧位姿估計值賦值給vtx,而後計算獲得在第二張圖片上的關鍵點的空間點座標xyz_trans.而後計算獲得最終的雅克比矩陣。
而後在protected裏定義灰度值函數。
這裏灰度值數據訪問形式爲
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
deltax爲floor(x),deltay=floor(y).1,x,y,xy分別爲data[0],data[1],data[image_->step],data[image_->step+1]
這個灰度值函數定義爲內聯函數,方便計算。
最終在public裏要定義類變量的類型和初值。這裏就是空間點座標,內參,灰度圖了。
而後看函數執行過程
先設置位姿的類型和初值,而後設置位姿的估計值形式爲se3quat形式,是由Tcw的旋轉矩陣和位移矩陣構成的。setId(0),而後把這個頂點添加到圖模型裏。
而後構造邊,邊是一個個進行構造的,每一個測量值均可以構造一個邊。因此構造一個循環
for (Measurement m:measurements)
{
設置邊的類型,邊的初值是由m.pos_wolrd,內參,新的灰度圖構造的。
邊鏈接的頂點就是pose,測量值就是m.grayscale,信息矩陣就是1*1的單位矩陣,id就是id++.而後圖模型把這條邊給加上。
記着頂點和邊都是指針,定義的時候要帶*號,賦初值的時候要帶new.
4.畫圖,這裏畫圖是把第一種圖片和以後的其餘圖片作對比,並且把圖片中的每一個關鍵點都用圓圈標註,兩張圖片對應的關鍵點連線。
要同時展現兩張圖片,img_show的行要是圖片行的兩倍,列仍是圖片的列。
cv::Mat img_show(color.rows*2,color.cols,CV_8UC3);
把第一種圖片prev_color複製到img_show的,從0,0到color.cols,color.rows.
而color則複製到img_show,從0.color.rows開始,行列爲color.cols,color.rows.
對於測量值的每一項,既是第一張圖片的關鍵點的空間點座標和灰度值組成的。
能夠知道在第一張圖片關鍵點的空間點座標,由此得第一張圖片關鍵點的像素座標pixel_prev,由Tcw能夠得關鍵點在第二張圖片的空間點座標,由此的特徵點在第二張圖片的像素座標pixel_now。
篩選若是像素座標小於0或者像素座標超出圖片行或列,跳出循環。
計算b,g,r值,由於用圓圈標註的時候須要用到b,g,r的值。
值都爲255*float ( rand() ) /RAND_MAX;
標註函數是cv::circle,第一種圖像標註的變量爲img_show,像素座標,8,cv::Scalar(b,g,r),2
第二張圖片標註用img_show,關鍵點在第二張圖片上的像素座標u,v可是v要加上函數color.rows,8,cv::Scalar(b,g,r),2.
連線,用cv::line函數,變量爲img_show,第一張圖片像素座標,第二張像素座標(v+color.rows),cv::Scalr(b,g,r),1.
展現圖像用cv::imshow()
cv::waitKey(0);
這是暫停,按任意鍵執行下個循環。函數