ORB-SLAM2 ---- IC_Angle函数

APS2023

于 2022-08-12 20:00:17 发布

阅读量1.1k

点赞数 3

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签： c++ 计算机视觉图像处理 orb-slam

本文链接：https://blog.csdn.net/qq_41694024/article/details/126306830

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

1.函数作用

计算特征点的方向，这里是返回角度作为方向

2. 灰度质心法数学基础

         在旋转方面，我们计算特征点附近的图像灰度质心。所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：
1．在一个小的图像块 $B$ 中，定义图像块的矩为

                                         $m_{p,q} = \sum_{(x,y)\epsilon B}^{}x^{p}y^{q}I(x,y) \ \ p,q={0,1}$

2．通过矩可以找到图像块的质心：

                                                         $C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$

3．连接图像块的几何中心 $O$ 与质心 $C$ ，得到一个方向向量 $\underset{oc}{\rightarrow}$ ，于是特征点的方向可以定义为
                                                     $\theta =arctan(m_{01}/m_{10})$

        通过以上方法，FAST角点便具有了尺度与旋转的描述，从而大大提升了其在不同图像之间表述的鲁棒性。所以在ORB中，把这种改进后的FAST称为Oriented FASR。

3.代码解析

3.1 源代码

/**
 * @brief 这个函数用于计算特征点的方向，这里是返回角度作为方向。
 * 计算特征点方向是为了使得提取的特征点具有旋转不变性。
 * 方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向
 * @param[in] image     要进行操作的某层金字塔图像
 * @param[in] pt        当前特征点的坐标
 * @param[in] u_max     图像块的每一行的坐标边界 u_max
 * @return float        返回特征点的角度，范围为[0,360)角度，精度为0.3°
 */
static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
	//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;

	//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
	//这条v=0中心线的计算需要特殊对待
    //后面是以中心行为对称轴，成对遍历行数，所以PATCH_SIZE必须是奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];

    int step = (int)image.step1();
	//注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
		//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
		// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
		//在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
			//得到需要进行加运算和减运算的像素灰度值
			//val_plus：在中心线下方x=u时的的像素灰度值
            //val_minus：在中心线上方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
			//在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
			//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
    }

    //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

3.2 源代码解析

首先我们要返回的是角度，即 $\theta =arctan(m_{01}/m_{10})$ ，于是我们的核心任务就是求y方向的矩 $m_{01}$ 和x方向的矩 $m_{10}$ 。

我们先拿取特征点所在的图像块特征点的中心点坐标灰度值的指针center，由于半径所在灰度质心圆的一条线的v=0，因此我们先算半径的 $m_{01}$ 与 $m_{10}$ ，显然，半径方向的y=0，于是只算x方向的矩即可。
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];
我们定义了每行的步长step，这个的作用是图像一行像素的字节数，举个例子，图像第一行第一个像素的位置 + 1*step 就能得到图像第二行第一个像素的位置。

我们从v=1到v= HALF_PATCH_SIZE 计算x、y方向的矩。下面我们来解释这段代码：
// Proceed over the two lines
		//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
		// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
		//在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
			//得到需要进行加运算和减运算的像素灰度值
			//val_plus：在中心线下方x=u时的的像素灰度值
            //val_minus：在中心线上方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
			//在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
			//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
这段代码我们从-d到d循环一次，即半径循环一次，一次计算对称的两行的矩，算法如下：

对于某次待处理的两个点：m_10 = Σ x*I(x,y) = x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
对于某次待处理的两个点：m_01 = Σ y*I(x,y) = y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
m_10 += u * (val_plus + val_minus);
这句代码是对每一行的上下两部分求x的矩。

由于y在每次循环取值相等，于是我们用临时变量v_sum 累加一行的灰度值，然后乘以y值就可以，简化了计算。

然后利用opencv的函数快速计算角度值返回给调用函数即可。