对极约束及极线搜索

最新推荐文章于 2024-09-06 22:53:27 发布

置顶 td092

最新推荐文章于 2024-09-06 22:53:27 发布

阅读量2.1k

点赞数

分类专栏：机器人

欢迎交流，有评论就更

本文链接：https://blog.csdn.net/weixin_38258767/article/details/106869609

版权

机器人专栏收录该内容

63 篇文章 9 订阅

订阅专栏

对极约束是指参考帧与当前帧光心与观测点组成的平面上的几何约束。
在这里插入图片描述
基于运动关系和共面约束，可以推出本质矩阵：

对于 $x^{'}$ ,可以看作是直线 $a x + b y + c = 0$ 上的点，直线参数由下面公式给出
$\begin{bmatrix}a\\b\\c \end{bmatrix}=Ex=E\begin{bmatrix}x'\\y'\\1 \end{bmatrix}$
这样将 $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