对极约束及极线搜索

对极约束是指参考帧与当前帧光心与观测点组成的平面上的几何约束。

基于运动关系和共面约束，可以推出本质矩阵：

对于$x^{\prime }$,可以看作是直线$ax+by+c=0$上的点，直线参数由下面公式给出
$$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=Ex=E\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]$$
这样将$x^{\prime }$代入直线方程，得到的结果即为对极误差。

在ORB_SLAM2中有：

bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
    // Epipolar line in second image l = x1'F12 = [a b c]
    const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
    const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
    const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);

    const float num = a*kp2.pt.x+b*kp2.pt.y+c;

    const float den = a*a+b*b;

    if(den==0)
        return false;

    const float dsqr = num*num/den;

    return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
}

利用对极约束，还可以进行极线搜索，在不使用描述子的情况下查找对应特征点：
下面代码未经过测试，仅供参考：

def epipolarSearch(ref_frame, cur_frame, ref_p):
    min_depth = 0.1
    max_depth= 100
    min_cur_p = ref_frame.K.dot(np.hstack(ref_frame.R, ref_frame.t)).dot(np.append(ref_p,[min_depth])
    min_cur_p /=  min_cur_p[2]
    max_cur_p = ref_frame.K.dot(np.hstack(ref_frame.R, ref_frame.t)).dot(np.append(ref_p,[max_depth])
	max_cur_p /=  max_cur_p[2]
    epopolar_dir = max_cur_p[:2] -min_cur_p[:2]
    epopolar_len = np.linalog.norm(epopolar_dir)
    epopolar_dir /= epopolar_len

    min_p = min_cur_p[:2]
    best_ncc = 0
    radius = 3
    if(not checkInlier(ref_p,ref_frame.size,radius)):
        return np.array([-1,-1])
    for l in np.arange(0,epopolar_len,1.44):
        cur_p  = min_p + l*epopolar_dir
        if(not checkInlier(cur_p,cur_frame.size,radius)):
            continue
        ncc = computeNCCScore(ref_frame, cur_frame, ref_p, cur_p,radius)
        if(ncc >best_ncc):
            best_cur_p = cur_p
            best_ncc = ncc
    if(best_ncc<0.85):
        return np.array([-1,-1])
    return best_cur_p
    

   def computeNCCScore(self,ref, curr, ptRef, ptCurr,radius):
       sum_up  = sum_down  = sum_down_1 = sum_down_2 = 0.0
       for r in range(-radius, radius+1):
           for c in range(-radius, radius+1):
               ref_val_uv = ref[ptRef[0]+r, ptRef[1]+c]/255
               cur_val_uv = curr[int(ptCurr[0])+r, int(ptCurr[1])+c]/255
               sum_up += ref_val_uv* cur_val_uv
               sum_down_1 += ref_val_uv * ref_val_uv
               sum_down_2 += cur_val_uv * cur_val_uv
       sum_down = math.sqrt(sum_down_1 * sum_down_2)
       ncc = sum_up / sum_down
       return ncc
        
def checkInlier(p,size,radius):
    if(p[0]<radius or p[0]>size[0]-radius-1 or p[1]<radius or p[1]>size[1]-radius-1):
        return False
    return True

参考：
http://www.cs.cmu.edu/~16385/s17/Slides/12.2_Essential_Matrix.pdf
http://www.bmva.org/bmvc/2000/papers/p55.pdf
https://github.com/yepeichu123/slam_module/tree/56af4e383d21a94a843268e56d7c8ce47a5a3cfa