对极约束及极线搜索

对极约束是指参考帧与当前帧光心与观测点组成的平面上的几何约束。
在这里插入图片描述
基于运动关系和共面约束,可以推出本质矩阵:
在这里插入图片描述
对于 x ′ x' x,可以看作是直线 a x + b y + c = 0 ax+by+c=0 ax+by+c=0上的点,直线参数由下面公式给出
[ a b c ] = E x = E [ x ′ y ′ 1 ] \begin{bmatrix}a\\b\\c \end{bmatrix}=Ex=E\begin{bmatrix}x'\\y'\\1 \end{bmatrix} abc=Ex=Exy1
这样将 x ′ x' x代入直线方程,得到的结果即为对极误差。

在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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值