ORB-SLAM2代码实现之灰度质心法学习记录

ORB-SLAM2代码实现之灰度质心法:

---

orb-slam2注释代码：https://github.com/electech6/ORBSLAM2_detailed_comments

实现思路：

目的：已知形心P,求解质心Q

 

 

 

 

1、为了保证旋转不变性，所以要在圆内计算；

2、为了在圆内计算，故要知道圆的边界，才能去索引；

3、为了知道圆的边界，在已知纵坐标的情况下，还需知道横坐标umax；

4、最终利用IC_Angle函数进行并行求解像素和

---

灰度质心原理计算方法：

---

计算图像金字塔特征点：

进入特征点提取器的构造函数：ORBextractor::ORBextractor

首先设置图像金字塔的各个参数：打算提取的特征点数目nfeatures；金字塔缩放系数scaleFactor；指定图像金字塔的层数nlevels。

        假设图像的长宽分别为L、W，缩放因子为s（0

        所需提取的特征点数量为N，则单位面积需提取的特征点数量为： 

           那么第 a 层应分配的特征点数量为：

第n层需要分的特征点：

float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));

 

---

计算umax：

umax以一个int向量方式进行存储，，具体含义表示为1/4圆的弦上点对应的横坐标。

先计算vmax：

vmax=cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);  //计算圆的最大行号，+1应该是把中间行也给考虑进去了

此时是45度角下，HALF_PATCH_SIZE表示半径。

根据勾股定理计算umax：

for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt(hp2 - v * v));

hp2表示半径平方，cvRound以及cvFloor表示近似取整。

---

计算特征点方向：

目的：算特征点方向是为了使得提取的特征点具有旋转不变性。

方法：灰度质心法：以几何中心和灰度质心的连线作为该特征点方向

进入计算特征点方向函数computeOrientation：

此函数主要就是调用了IC_Angle函数，此用于计算特征点方向，返回一个角度，范围为[0,360)角度，精度为0.3°。

---

IC_Angle：

输入：要进行操作的某层金字塔图像image、 当前特征点的坐标即形心pt、图像块的每一行的坐标边界 u_max。

IC_Angle加速计算灰度值：

参考https://www.cnblogs.com/wall-e2/p/8057448.html

 

先算出中间红线这一行的，然后在平行于x轴算出和，一次计算相当于图像中的同个颜色的两个line。

//假设每次处理的两个点坐标，中心线上方为(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))

具体代码实现中：

static float IC_Angle(const Mat& image, Point2f pt,  const vector & u_max)
{
	//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;

	//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at (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];

    // Go line by line in the circular patch  
	//这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279]
    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);
}

其中定义了一个指针center指向这个特征点所在的图像块的中心点坐标灰度值；

v_sum 为计算的中间值，同时v在循环中是个定值，所以放在循环外计算。

 

---