導讀

極限校正的實現原理方法以及代碼詳解。

為什么要做極線校正？

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

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

做極線校正我們需要知道哪些基礎的立體視覺相關的基礎知識？

1. 齊次坐標，剛體變換

齊次坐標

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

剛體變換

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

即：

用齊次坐標表示：

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

3個旋轉3個平移共6個自由度；

用途：

計算一個剛體經過旋轉和平移后的新坐標 ；

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

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

圖像坐標系

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

用矩陣的表示形式:

逆關系:

相機與世界坐標系

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

世界坐標系與相機坐標系轉換關系：

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

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

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

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

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

稱為歸一化焦距,只與相機內部參數有關, 被稱為相機內參;由相機相對世界坐標系的方位決定, 被稱為相機外參。確定相機內外參數被稱為相機標定。

4.對極幾何

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

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

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

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

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

極限校正怎么實現？

1.三角測量原理

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

利用三角形關系我們很容易推出Z值：

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

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

視差與深度圖關系：

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

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

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

2.極線校正原理

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

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

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

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

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

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

最終得到的旋轉矩陣:

則左右兩個圖像新的旋轉矩陣:

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)
//新的內參
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
//新的選擇從校正前的相機坐標系轉到校正后的相機坐標系
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

極線校正后的視差圖轉校正前的深度圖怎么轉,代碼怎么實現？

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

則:

把上式寫成矩陣形式:

式中定義如下:

將校正后的坐標系下深度圖轉換到校正前的相機坐標系下

將校正后的坐標系下深度圖轉換到校正前的相機坐標系下

已知：校正前的相機坐標系下坐標轉到校正后相機坐標系下坐標轉換關系如下:

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

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

即：

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

已知：

則：

代碼實現

轉換矩陣求解

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
//調整校正后的相機矩陣，使兩個源圖像的公共區域都包含在校正后的圖像中
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)
//計算校正的單應性矩陣（描述的是兩個圖像像素坐標的轉換矩陣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)
//校正圖像,就是利用單應性矩陣，把原圖像每個像素坐標轉換到校正的圖像下。
rectifiedImage1.create(size1);
cv::warpPerspective(image1.image,rectifiedImage1,H1,rectifiedImage1.size());
rectifiedImage2.create(size2);
cv::warpPerspective(image2.image,rectifiedImage2,H2,rectifiedImage2.size());

//markvalidregionscoveredbytherectifiedimages
//標記正確圖像覆蓋的有效區域
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:
?//?根據depth=Bf/d的關系，計算投影矩陣Q將校正的視差圖轉到為校正的深度圖。
?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
?//?計算將視差從校正到原始圖像深度轉換的Q值
?Matrix4x4?P(Matrix4x4::IDENTITY);
?cv::Mat(4,4,cv::DataType::type,P.val)(cv::Rect(0,0,3,3)));
Q=P*Q;
returntrue;
}

根據求解的轉換矩陣進行視差圖和深度圖相互轉換

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

	    責任編輯：彭菁