極限校正的實現(xiàn)原理方法有哪些

導(dǎo)讀

極限校正的實現(xiàn)原理方法以及代碼詳解。

為什么要做極線校正？

三維重建是通過雙目立體匹配實現(xiàn)的如圖1，通過匹配空間中點在兩個圖像中的投影點，再根據(jù)三角關(guān)系得到P的Z值。

我們雙目相機拍攝的時候?qū)嶋H情況下如下圖a,兩個圖像做匹配時如我們圖中藍色箭頭指示的匹配點那樣，需要在全圖中進行查找。但是如果我們對相機進行校正，使得它們成像面平行且行對齊如下圖b，匹配點在同一行。那么我們只需在同行上查找，大大節(jié)約時間。因此，極線校正目的是對兩幅圖像的二維匹配搜索變成一維，節(jié)省計算量，排除虛假匹配點。

做極線校正我們需要知道哪些基礎(chǔ)的立體視覺相關(guān)的基礎(chǔ)知識？

1. 齊次坐標，剛體變換

齊次坐標

用n+1維矢量表示n維矢量，主要用途是可以方便矩陣運算。（參考下式）

剛體變換

假設(shè)歐式空間中一點, 在兩個坐標系中的坐標分別為和, 那么圖中變換公式:

即：

用齊次坐標表示：

剛性物體無論位置和方向發(fā)生變換還是在不同坐標系下觀察同一個物體，它的形狀和大小都保持不變；

3個旋轉(zhuǎn)3個平移共6個自由度；

用途：

計算一個剛體經(jīng)過旋轉(zhuǎn)和平移后的新坐標 ；

計算同一剛體在不同坐標系下的坐標。

2.圖像坐標系，相機坐標系，世界坐標系

圖像坐標系

上圖中,是表示以像素為單位的圖像坐標系的坐標,是以為單位的圖像坐標系的坐標,是相機光軸與圖像的交點其對應(yīng)的像素坐標, 每個像素在軸和軸對應(yīng)的 物理尺寸。兩者轉(zhuǎn)換關(guān)系如下:

用矩陣的表示形式:

逆關(guān)系:

相機與世界坐標系

為相機光心,軸與圖像的軸平行,軸為相機光軸與相機圖像平面垂直交點就是圖像坐標原點。與組成的直角坐標系為相機坐標系,為焦距。

世界坐標系與相機坐標系轉(zhuǎn)換關(guān)系：

式中為的正交單位矩陣,為三維平移向量,為矩陣。

3.相機投影模型（針孔投影模型）

把圖像平面放到針孔前方（數(shù)學(xué)上等價，計算更簡單）：

當已知圖像點p時，由針孔成像模型，任何位于射線OP上的空間點的圖像點都是p點，因此空間點不是唯一確定的。

投影關(guān)系 (從世界坐標系到圖像坐標系)

稱為歸一化焦距,只與相機內(nèi)部參數(shù)有關(guān), 被稱為相機內(nèi)參;由相機相對世界坐標系的方位決定, 被稱為相機外參。確定相機內(nèi)外參數(shù)被稱為相機標定。

4.對極幾何

極點: 極點: 右相機坐標原點在左像平面上的像; 極點:左相機坐標原點在右像平面上的像

極平面：由兩個相機坐標原點、和物點組成的平面

極線: 極平面與兩個像平面的交線

極線約束：給定圖像上的一個特征, 它在另一幅圖像上的匹配視圖一定在對應(yīng)的極線上, 即已知則它對應(yīng)在右圖的匹配點一定在極線上反之亦然。

極線約束給出了對應(yīng)點重要的約束條件，它將對應(yīng)點匹配從整幅圖像中查找壓縮到一條線上查找，大大減小了搜索范圍，對對應(yīng)點的匹配起指導(dǎo)作用。

極限校正怎么實現(xiàn)？

1.三角測量原理

假設(shè)我們兩臺攝像機像平面精準位于同一平面上，且行對齊，兩個光軸嚴格平行。

利用三角形關(guān)系我們很容易推出Z值：

如果主點坐標相同則可簡化為:

因為視差，且為我們的深度值, 故:

視差與深度圖關(guān)系：

視差與深度成反比，視差接近0時，微小的視差變化會產(chǎn)生較大的深度變化

當視差較大時，微小的視差變化幾乎不會引起深度多大的變化

因此，立體視覺系統(tǒng)僅物體距離相機較近時具有較高的深度精度

2.極線校正原理

校正過程：將相機在數(shù)學(xué)上對準到同一觀察平面上，使得相機上像素行是嚴格對齊的

校正目的：對兩幅圖像的二維匹配搜索變成一維，節(jié)省計算量，排除虛假匹配點

3.校正算法步驟：（參考文獻A compact algorithm for rectification of stereo pairs）

為了將攝相機極點變換到無窮遠使得極線水平對準, 我們創(chuàng)建一個旋轉(zhuǎn)矩陣, 首先是軸的旋轉(zhuǎn), 我們將軸旋轉(zhuǎn)到與基線相同的方向即旋轉(zhuǎn)后新軸就是, 旋轉(zhuǎn)向量:

確定旋轉(zhuǎn)后的新軸, 只需滿足與正交即可：選則與主光軸和相交的方向：

新的軸, 與垂直且滿足右手定則:

最終得到的旋轉(zhuǎn)矩陣:

則左右兩個圖像新的旋轉(zhuǎn)矩陣:

4.校正后，baseline 計算:

下式中是指左相機光心在右相機坐標系下的坐標（也等于與右相機光心的距離），t 求解公式的意義是左相機光心在右相機校正后的坐標系下的坐標（也等于與右相機校正后的坐標系下光心的距離) 。故baseline就是軸方向的距離。

則

極線校正代碼怎么寫？

//see:"Acompactalgorithmforrectificationofstereopairs",A.Fusiello,E.Trucco,andA.Verri,2000
//極線校正
REALCamera::StereoRectifyFusiello(constcv::Size&size1,constCamera&camera1,constcv::Size&size2,constCamera&camera2,Matrix3x3&R1,Matrix3x3&R2,Matrix3x3&K1,Matrix3x3&K2)
{
//computerelativepose
//計算相對位姿
RMatrixposeR;
CMatrixposeC;
ComputeRelativePose(camera1.R,camera1.C,camera2.R,camera2.C,poseR,poseC);

//newxaxis(baseline,fromC1toC2)
//新的x軸，基線方向
constPoint3v1(camera2.C-camera1.C);
//newyaxes(orthogonaltooldzandnewx)
//新的y軸，垂直舊的Z軸（光軸）和新的X軸
constPoint3v2(camera1.Direction().cross(v1));
//newzaxes(nochoice,orthogonaltobaselineandy)
//新的Z軸，垂直上面兩個新軸
constPoint3v3(v1.cross(v2));

//newextrinsic(translationunchanged)
//新的外參，平移不變
RMatrixR;
R.SetFromRowVectors(normalized(v1),normalized(v2),normalized(v3));

//newintrinsic(arbitrary)
//新的內(nèi)參
K1=camera1.K;K1(0,1)=0;
K2=camera2.K;K2(0,1)=0;
K1(1,1)=K2(1,1)=(camera1.K(1,1)+camera2.K(1,1))/2;

//newrotations
//新的選擇從校正前的相機坐標系轉(zhuǎn)到校正后的相機坐標系
R1=R*camera1.R.t();
R2=R*camera2.R.t();
//計算新的基線距離
constPoint3t(R2*(poseR*(-poseC)));
ASSERT(ISEQUAL(-t.x,norm(v1))&&ISZERO(t.y)&&ISZERO(t.z));
returnt.x;
}//StereoRectifyFusiello

極線校正后的視差圖轉(zhuǎn)校正前的深度圖怎么轉(zhuǎn),代碼怎么實現(xiàn)？

視差圖是校正后的坐標系下得到的值,首先將其轉(zhuǎn)換為校正后坐標系下的深度圖

則:

把上式寫成矩陣形式:

式中定義如下:

將校正后的坐標系下深度圖轉(zhuǎn)換到校正前的相機坐標系下

將校正后的坐標系下深度圖轉(zhuǎn)換到校正前的相機坐標系下

已知：校正前的相機坐標系下坐標轉(zhuǎn)到校正后相機坐標系下坐標轉(zhuǎn)換關(guān)系如下:

則已知校正后的求校正前的公式如下:

再將其投影到圖像坐標系下:

即：

將上兩步合并得到校正后坐標系下的視差圖（與校正前的坐標系下的深度圖的轉(zhuǎn)換矩陣

已知：

則：

代碼實現(xiàn)

轉(zhuǎn)換矩陣求解

boolImage::StereoRectifyImages(constImage&image1,constImage&image2,constPoint3fArr&points1,constPoint3fArr&points2,Image8U3&rectifiedImage1,Image8U3&rectifiedImage2,Image8U&mask1,Image8U&mask2,Matrix3x3&H,Matrix4x4&Q)
{
ASSERT(image1.IsValid()&&image2.IsValid());
ASSERT(image1.GetSize()==image1.image.size()&&image2.GetSize()==image2.image.size());
ASSERT(points1.size()&&points2.size());

#if0
{//displayprojectionpairs
std::vectormatches1,matches2;
FOREACH(i,points1){
matches1.emplace_back(reinterpret_cast(points1[i]));
matches2.emplace_back(reinterpret_cast(points2[i]));
}
RECTIFY::DrawMatches(const_cast(image1.image),const_cast(image2.image),matches1,matches2);
}
#endif

//computerectification
//校正計算
Matrix3x3K1,K2,R1,R2;
#if0
constREALt(Camera::StereoRectify(image1.GetSize(),image1.camera,image2.GetSize(),image2.camera,R1,R2,K1,K2));
#elif1
constREALt(Camera::StereoRectifyFusiello(image1.GetSize(),image1.camera,image2.GetSize(),image2.camera,R1,R2,K1,K2));
#else
Posepose;
ComputeRelativePose(image1.camera.R,image1.camera.C,image2.camera.R,image2.camera.C,pose.R,pose.C);
cv::MatP1,P2;
cv::stereoRectify(image1.camera.K,cv::noArray(),image2.camera.K,cv::noArray(),image1.GetSize(),pose.R,Vec3(pose.GetTranslation()),R1,R2,P1,P2,Q,0/*cv::CALIB_ZERO_DISPARITY*/,-1);
K1=P1(cv::Rect(0,0,3,3));
K2=P2(cv::Rect(0,0,3,3));
constPoint3_t(R2*pose.GetTranslation());
ASSERT((ISZERO(_t.x)||ISZERO(_t.y))&&ISZERO(_t.z));
constREALt(ISZERO(_t.x)?_t.y:_t.x);
#if0
cv::Matmap1,map2;
cv::initUndistortRectifyMap(image1.camera.K,cv::noArray(),R1,K1,image1.GetSize(),CV_16SC2,map1,map2);
cv::remap(image1.image,rectifiedImage1,map1,map2,cv::INTER_CUBIC);
cv::initUndistortRectifyMap(image2.camera.K,cv::noArray(),R2,K2,image1.GetSize(),CV_16SC2,map1,map2);
cv::remap(image2.image,rectifiedImage2,map1,map2,cv::INTER_CUBIC);
return;
#endif
#endif
if(ISZERO(t))
returnfalse;

//adjustrectifiedcameramatricessuchthattheentireareacommontobothsourceimagesiscontainedintherectifiedimages
//調(diào)整校正后的相機矩陣，使兩個源圖像的公共區(qū)域都包含在校正后的圖像中
cv::Sizesize1(image1.GetSize()),size2(image2.GetSize());
if(!points1.empty())
Camera::SetStereoRectificationROI(points1,size1,image1.camera,points2,size2,image2.camera,R1,R2,K1,K2);
ASSERT(size1==size2);

//computerectificationhomography(fromoriginaltorectifiedimage)
//計算校正的單應(yīng)性矩陣（描述的是兩個圖像像素坐標的轉(zhuǎn)換矩陣H[u,v,1]^t=[u',v',1]^t）(從原始圖像到校正圖像)
constMatrix3x3H1(K1*R1*image1.camera.GetInvK());H=H1;
constMatrix3x3H2(K2*R2*image2.camera.GetInvK());

#if0
{//displayepipolarlinesbeforeandafterrectification
Posepose;
ComputeRelativePose(image1.camera.R,image1.camera.C,image2.camera.R,image2.camera.C,pose.R,pose.C);
constMatrix3x3F(CreateF(pose.R,pose.C,image1.camera.K,image2.camera.K));
std::vectormatches1,matches2;
#if1
FOREACH(i,points1){
matches1.emplace_back(reinterpret_cast(points1[i]));
matches2.emplace_back(reinterpret_cast(points2[i]));
}
#endif
RECTIFY::DrawRectifiedImages(image1.image.clone(),image2.image.clone(),F,H1,H2,matches1,matches2);
}
#endif

//rectifyimages(applyhomographies)
//校正圖像,就是利用單應(yīng)性矩陣，把原圖像每個像素坐標轉(zhuǎn)換到校正的圖像下。
rectifiedImage1.create(size1);
cv::warpPerspective(image1.image,rectifiedImage1,H1,rectifiedImage1.size());
rectifiedImage2.create(size2);
cv::warpPerspective(image2.image,rectifiedImage2,H2,rectifiedImage2.size());

//markvalidregionscoveredbytherectifiedimages
//標記正確圖像覆蓋的有效區(qū)域
structCompute{
staticvoidMask(Image8U&mask,constcv::Size&sizeh,constcv::Size&size,constMatrix3x3&H){
mask.create(sizeh);
mask.memset(0);
std::vectorcorners(4);
corners[0]=Point2f(0,0);
corners[1]=Point2f((float)size.width,0);
corners[2]=Point2f((float)size.width,(float)size.height);
corners[3]=Point2f(0,(float)size.height);
cv::perspectiveTransform(corners,corners,H);
std::vector>contours(1);
for(inti=0;i<4;?++i)
????contours.front().emplace_back(ROUND2INT(corners[i]));
???cv::drawContours(mask,?contours,?0,?cv::Scalar(255),?cv::FILLED);
??}
?};
?Compute::Mask(mask1,?size1,?image1.GetSize(),?H1);
?Compute::Mask(mask2,?size2,?image2.GetSize(),?H2);

?//?from?the?formula?that?relates?disparity?to?depth?as?z=B*f/d?where?B=-t?and?d=x_l-x_r
?//?and?the?formula?that?converts?the?image?projection?from?right?to?left?x_r=K1*K2.inv()*x_l
?//?compute?the?inverse?projection?matrix?that?transforms?image?coordinates?in?image?1?and?its
?//?corresponding?disparity?value?to?the?3D?point?in?camera?1?coordinates?as:
?//?根據(jù)depth=Bf/d的關(guān)系，計算投影矩陣Q將校正的視差圖轉(zhuǎn)到為校正的深度圖。
?ASSERT(ISEQUAL(K1(1,1),K2(1,1)));
?Q?=?Matrix4x4::ZERO;
?//???Q?*?[x,?y,?disparity,?1]?=?[X,?Y,?Z,?1]?*?w
?ASSERT(ISEQUAL(K1(0,0),K2(0,0))?&&?ISZERO(K1(0,1))?&&?ISZERO(K2(0,1)));
?Q(0,0)?=?Q(1,1)?=?REAL(1);
?Q(0,3)?=?-K1(0,2);
?Q(1,3)?=?-K1(1,2);
?Q(2,3)?=??K1(0,0);
?Q(3,2)?=?-REAL(1)/t;
?Q(3,3)?=??(K1(0,2)-K2(0,2))/t;

?//?compute?Q?that?converts?disparity?from?rectified?to?depth?in?original?image
?//?計算將視差從校正到原始圖像深度轉(zhuǎn)換的Q值
?Matrix4x4?P(Matrix4x4::IDENTITY);
?cv::Mat(4,4,cv::DataType::type,P.val)(cv::Rect(0,0,3,3)));
Q=P*Q;
returntrue;
}

根據(jù)求解的轉(zhuǎn)換矩陣進行視差圖和深度圖相互轉(zhuǎn)換

/**
*@brief 根據(jù)原圖的深度圖計算校正后的圖像的視差圖，視差圖事先根據(jù)有效尺寸分配好。
*
*@param[in]depthMap原圖的深度圖
*@param[in] invH 轉(zhuǎn)換矩陣：把校正圖的像素坐標轉(zhuǎn)換到原圖
*@param[in]invQ轉(zhuǎn)換矩陣把[x*zy*zz1]*winoriginalimagecoordinates（z即為depth）轉(zhuǎn)到[x'y'disparity1]inrectifiedcoordinates
*@param[in] subpixelSteps 亞像素精度，如果是4則對于精度是0.25，主要是視差值存儲的都是整型。所以得到的視差會乘這個值轉(zhuǎn)成整型。具體轉(zhuǎn)回float真值時會除掉這個數(shù)
*@param[in]disparityMap視差圖
*/
voidSemiGlobalMatcher::Depth2DisparityMap(constDepthMap&depthMap,constMatrix3x3&invH,constMatrix4x4&invQ,DisparitysubpixelSteps,DisparityMap&disparityMap)
{
autopixel=[&](int,intr,intc){
//rc加half窗口的原因是視差圖是有效像素開始的起始是從（halfWindowSizeX，halfWindowSizeY）開始的而非（0，0）
constImageRefx(c+halfWindowSizeX,r+halfWindowSizeY);Point2fu;
//把校正圖像上的像素坐標轉(zhuǎn)到原圖坐標上
ProjectVertex_3x3_2_2(invH.val,x.ptr(),u.ptr());
floatdepth,disparity;
//取深度值depth并轉(zhuǎn)到視差上
if(!depthMap.sampleSafe(depth,u,[](Depthd){returnd>0;})||!Image::Depth2Disparity(invQ,u,depth,disparity))
disparityMap(r,c)=NO_DISP;
else
disparityMap(r,c)=(Disparity)ROUND2INT(disparity*subpixelSteps);
};
ASSERT(threads.IsEmpty());
if(!threads.empty()){
volatileThread::safe_tidxPixel(-1);
FOREACH(i,threads)
threads.AddEvent(newEVTPixelProcess(disparityMap.size(),idxPixel,pixel));
WaitThreadWorkers(threads.size());
}else
for(intr=0;r

	    責任編輯：彭菁