導(dǎo)讀

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

為什么要做極線校正？

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

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

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

1. 齊次坐標(biāo)，剛體變換

齊次坐標(biāo)

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

剛體變換

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

即：

用齊次坐標(biāo)表示：

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

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

用途：

計(jì)算一個(gè)剛體經(jīng)過(guò)旋轉(zhuǎn)和平移后的新坐標(biāo) ；

計(jì)算同一剛體在不同坐標(biāo)系下的坐標(biāo)。

2.圖像坐標(biāo)系，相機(jī)坐標(biāo)系，世界坐標(biāo)系

圖像坐標(biāo)系

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

用矩陣的表示形式:

逆關(guān)系:

相機(jī)與世界坐標(biāo)系

為相機(jī)光心,軸與圖像的軸平行,軸為相機(jī)光軸與相機(jī)圖像平面垂直交點(diǎn)就是圖像坐標(biāo)原點(diǎn)。與組成的直角坐標(biāo)系為相機(jī)坐標(biāo)系,為焦距。

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

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

3.相機(jī)投影模型（針孔投影模型）

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

當(dāng)已知圖像點(diǎn)p時(shí)，由針孔成像模型，任何位于射線OP上的空間點(diǎn)的圖像點(diǎn)都是p點(diǎn)，因此空間點(diǎn)不是唯一確定的。

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

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

4.對(duì)極幾何

極點(diǎn): 極點(diǎn): 右相機(jī)坐標(biāo)原點(diǎn)在左像平面上的像; 極點(diǎn):左相機(jī)坐標(biāo)原點(diǎn)在右像平面上的像

極平面：由兩個(gè)相機(jī)坐標(biāo)原點(diǎn)、和物點(diǎn)組成的平面

極線: 極平面與兩個(gè)像平面的交線

極線約束：給定圖像上的一個(gè)特征, 它在另一幅圖像上的匹配視圖一定在對(duì)應(yīng)的極線上, 即已知?jiǎng)t它對(duì)應(yīng)在右圖的匹配點(diǎn)一定在極線上反之亦然。

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

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

1.三角測(cè)量原理

假設(shè)我們兩臺(tái)攝像機(jī)像平面精準(zhǔn)位于同一平面上，且行對(duì)齊，兩個(gè)光軸嚴(yán)格平行。

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

如果主點(diǎn)坐標(biāo)相同則可簡(jiǎn)化為:

因?yàn)橐暡?，且為我們的深度? 故:

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

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

當(dāng)視差較大時(shí)，微小的視差變化幾乎不會(huì)引起深度多大的變化

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

2.極線校正原理

校正過(guò)程：將相機(jī)在數(shù)學(xué)上對(duì)準(zhǔn)到同一觀察平面上，使得相機(jī)上像素行是嚴(yán)格對(duì)齊的

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

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

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

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

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

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

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

4.校正后，baseline 計(jì)算:

下式中是指左相機(jī)光心在右相機(jī)坐標(biāo)系下的坐標(biāo)（也等于與右相機(jī)光心的距離），t 求解公式的意義是左相機(jī)光心在右相機(jī)校正后的坐標(biāo)系下的坐標(biāo)（也等于與右相機(jī)校正后的坐標(biāo)系下光心的距離) 。故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
//計(jì)算相對(duì)位姿
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軸，垂直上面兩個(gè)新軸
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
//新的選擇從校正前的相機(jī)坐標(biāo)系轉(zhuǎn)到校正后的相機(jī)坐標(biāo)系
R1=R*camera1.R.t();
R2=R*camera2.R.t();
//計(jì)算新的基線距離
constPoint3t(R2*(poseR*(-poseC)));
ASSERT(ISEQUAL(-t.x,norm(v1))&&ISZERO(t.y)&&ISZERO(t.z));
returnt.x;
}//StereoRectifyFusiello

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

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

則:

把上式寫成矩陣形式:

式中定義如下:

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

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

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

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

再將其投影到圖像坐標(biāo)系下:

即：

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

已知：

則：

代碼實(shí)現(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
//校正計(jì)算
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)整校正后的相機(jī)矩陣，使兩個(gè)源圖像的公共區(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)
//計(jì)算校正的單應(yīng)性矩陣（描述的是兩個(gè)圖像像素坐標(biāo)的轉(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)性矩陣，把原圖像每個(gè)像素坐標(biāo)轉(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
//標(biāo)記正確圖像覆蓋的有效區(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)系，計(jì)算投影矩陣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
?//?計(jì)算將視差從校正到原始圖像深度轉(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)換矩陣進(jìn)行視差圖和深度圖相互轉(zhuǎn)換

/**
*@brief 根據(jù)原圖的深度圖計(jì)算校正后的圖像的視差圖，視差圖事先根據(jù)有效尺寸分配好。
*
*@param[in]depthMap原圖的深度圖
*@param[in] invH 轉(zhuǎn)換矩陣：把校正圖的像素坐標(biāo)轉(zhuǎn)換到原圖
*@param[in]invQ轉(zhuǎn)換矩陣把[x*zy*zz1]*winoriginalimagecoordinates（z即為depth）轉(zhuǎn)到[x'y'disparity1]inrectifiedcoordinates
*@param[in] subpixelSteps 亞像素精度，如果是4則對(duì)于精度是0.25，主要是視差值存儲(chǔ)的都是整型。所以得到的視差會(huì)乘這個(gè)值轉(zhuǎn)成整型。具體轉(zhuǎn)回float真值時(shí)會(huì)除掉這個(gè)數(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;
//把校正圖像上的像素坐標(biāo)轉(zhuǎn)到原圖坐標(biāo)上
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

	    責(zé)任編輯：彭菁