ORBSLAM2-灰度质心法求旋转不变性

---

代码

当使用四叉树的策略，将ORB特征点均匀的分布在在图像中后，接下来就需要进行计算方向，因为FAST角点与Brief描述子是不具备旋转不变性的，因此用灰度质心法计算旋转不变性。

const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];//定义每层patch_size的缩放，，这边就是在前面系统构造函数中所画的圆的直径，因为在图像金字塔中，每层的图像大小是不一样的，因此计算的圆的大小也是不一样的，需要进行比例缩放

        // Add border to coordinates and scale information
        const int nkps = keypoints.size();//获取保留下来的特征点的数目
        for(int i=0; i<nkps ; i++)
        { 
        //遍历每个特征点，恢复到扩充图层的坐标
            keypoints[i].pt.x+=minBorderX;
            keypoints[i].pt.y+=minBorderY;
            keypoints[i].octave=level;//获取图像在金字塔的哪层
            keypoints[i].size = scaledPatchSize;//每层对应patchsize的大小
        }
    }

    // compute orientations开始计算方向
    for (int level = 0; level < nlevels; ++level)
        computeOrientation(mvImagePyramid[level], //对应的图层图像
        allKeypoints[level],//这个图层中提取并且保留下来的特征电容器
         umax);//patch的坐标边界
}

进入computeOrientation函数

static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)//这边开始遍历每一个特征点
    {//调用IC——Angle来计算
        keypoint->angle = IC_Angle(image, keypoint->pt, umax);
    }
}

进入到IC_Angle函数
关于灰度质心法如下：

图1
如图1所示，p是特征点，以p为圆心PATCH_SIZE为直径画圆，那么P就是这个圆的几何中心，然后计算圆内的灰度，算出灰度质心Q，具体公式如14讲上面：
遍历[-HALF_PATCH_SIZE,HALF_PATCH_SIZE]上的像素点，算出整个圆的灰度值m00.

然后计算出x轴与y轴方向上的灰度，主要通过权重来算出：

例如m10中x表示的是x轴方向上的像素坐标，I(x，y)表示的是每个点灰度值。
算出x轴与y轴上的灰度值，将其与整体灰度相除，就能算出图像在x轴与y轴的灰度质心点，即整个图像的灰度质心点Q。

然后，利用arctan算出角度，就可以表示该特征点的方向。

IC_Angle函数加速

参考连接 图2
对于遍历整个圆中的坐标点是有点慢的，可以对其进行加速。
先算出中间红线这一行的m10，然后在平行于x轴算出m10和m01，一次计算相当于图像中的同个颜色的两个line

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{

    int m_01 = 0, m_10 = 0;//表示灰度质心里面的x与y轴上的灰度

    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));//特征点所在的灰度值的指针  

    // Treat the center line differently, v=0//开始计算，原理可以看图2，首先计算第0行的，也就是红色线条那行，比较特殊
    
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)//这边遍历【-u，u】，也就是平行于x轴
        m_10 += u * center[u];//x*I（x，y）

    // Go line by line in the circuI853lar patch
    int step = (int)image.step1();//图像一行中包含的字节总数 ，方便取对称的行数
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)//v是正的，也就是只遍历上半部分圆，
    {
        // Proceed over the two lines
        int v_sum = 0;
        int d = u_max[v];//横坐标的最大范围
        // 假设每次处理的两个点坐标，中心线上方为(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)//但是u却取了整个，即【-u，u】，遍历两行
        {
            int val_plus = center[u + v*step], val_minus = center[u - v*step];//v相对于center的位移，也就是y与-y
            v_sum += (val_plus - val_minus);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }

    return fastAtan2((float)m_01, (float)m_10);//返回角度
}