slam中特征点归一化原因以及方法
简述
在计算H 或者 F矩阵的时候需要对特征点进行坐标变换,称之为归一化。
原因
前辈发现计算单应矩阵时变换特征点的坐标会得到更好的效果,包括坐标的平移和尺度缩放,并且这一步骤必须放在DLT之前。DLT之后再还原到原坐标系。
书本指出归一化与条件数确切的说是DTL矩阵A的第一个和倒数第二个奇异值的比例有关。有充分证据表明在精确数据和无限精度的算术运算条件下,归一化并不起作用,但是有噪声存在时解将偏离其正确结果。
个人推测:类似于机器学习中需要对数据进行归一化,减少数据因为尺度变化过大异常值等的原因影响结果。
步骤
1.将点进行平移使其形心(x,y的均值)位于原点。
2.对点进行缩放使特征点到原点的距离为根号2,即所有点“平均”位于(1,1,1)
3.对两幅图进行独立的上述变换
参考自《计算机视觉中的多视图几何》 3.4.4
代码实现 from ORBslam2
/** * @brief 归一化特征点到同一尺度(作为normalize DLT的输入) * * [x' y' 1]' = T * [x y 1]' \n * 归一化后x', y'的均值为0,sum(abs(x_i'-0))=1,sum(abs((y_i'-0))=1 * * @param vKeys 特征点在图像上的坐标 * @param vNormalizedPoints 特征点归一化后的坐标 * @param T 将特征点归一化的矩阵 左乘 */ void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) { //第一步得到所有特征点的均值,并将所有点的均值为0 float meanX = 0; float meanY = 0; for (int i = 0; i < vKeys.size(); i++) { meanX += vKeys[i].pt.x; meanY += vKeys[i].pt.y; } meanX /= vKeys.size(); meanY /= vKeys.size(); //第二步将所有点到原点的距离为根号2 float meanDevX = 0; float meanDevY = 0; for (int i = 0; i < vKeys.size(); i++) { vNormalizedPoints[i].x = vKeys[i].pt.x - meanX; vNormalizedPoints[i].y = vKeys[i].pt.y - meanY; meanDevX += fabs(vNormalizedPoints[i].x); //fabs是求一个实数的绝对值 点到原点距离的累加 meanDevY += fabs(vNormalizedPoints[i].y); } meanDevX /= vKeys.size(); //点到原点距离的平均值 meanDevY /= vKeys.size(); for (int i = 0; i < vKeys.size(); i++) { vNormalizedPoints[i].x /= meanDevX; vNormalizedPoints[i].y /= meanDevY; } //用于还原特征点到原始的坐标系,获得矩阵 |sX 0 -meanx*sX| 用于取逆 x 快速还原 // |0 sY -meany*sY| * y // |0 0 1 | 1 float sX = 1.0 / meanDevX; float sY = 1.0 / meanDevY; T = Mat::eye(3, 3, CV_32F); T.at<float>(0, 0) = sX; T.at<float>(0, 2) = -meanX * sX; T.at<float>(1, 1) = sY; T.at<float>(1, 2) = -meanY * sY; T.at<float>(2, 2) = 1; }