RANSAC鲁棒估计算法

在VSLAM中，我们假定给出的对应集{x-x'},然后去计算相机的运动估计，但是由于噪声的影响，错误的匹配导致很差的估算效果

采用2种方法减少这个错误匹配影响，选择那些正确的匹配进行估计（RANSAC）和降低那些错误匹配的权重（鲁棒核函数）

两者区别是：

RANSAC方法进行模型估计，实际上分了两步，首先选出局内点，然后再进行一步优化。鲁棒核函数只需要一步，直接优化求解模型参数，通过降低外点的权重，来降低错误数据的影响。

这里只介绍RANSAC随机采样一致算法

这种方法的目的是，从所有数据中选择正确的数据。

基本的概念：

点：每一个数据，SLAM里指的是匹配的点对
野值/外点：错误的点
内点：正确的点
内点集：内点的集合
外点集：外点的集合
模型：带估计的参数
s ：估计模型所需要的最小点数
S：所有的点形成的点集

 显然，内点集就是我们想要找的正确数据。RANSAC的想法是从点集中随机的选择出 s个点，估计出一个模型，查看剩余点是否符合这个模型。由于是随机选择，一次可能难以找到正确的模型，因此需要多次才可以。

完整的RANSAC方法过程：

目标：

       一个模型与含有野值的数据集S的鲁邦拟合。

算法：

       （1）随机的从S中选择s个数据点（不一定是一个数据，可能是一对数据）组成一个样本作为模型的一个示例

        （2）确定在模型距离阈值t内的数据集S，Si为采样一致性集并定义S的内点集

        （3）如果Si的大小（内点的数量）大于某个阈值T，则使用Si的所有点重新估计模型并且结束估计

        （4）如果Si的大小（内点的数量）小于某个阈值T，则选择新的子集并且重新上面过程

        （5）经过N次试验选择最大的内点集Si，并用Si所有点估计模型

 算法三个参数; 距离阈值t,采样数量N, 内点数量阈值T

 距离阈值t:

 我们希望选择距离阈值t使点成为内点的概率是a,该计算需要知道内点到模型的距离的概率分布，在实际过程中距离阈值通过经验选择，根据模型，每个点可以计算出一个距离的平方  ，利用阈值 就可以识别该点为内点还是外点了。 的选择就要根据具体模型去选了，一般直线方程估计中采用点到线的距离的平方，基本矩阵估计中也使用点到极线的距离的平方，单应和相机内参估计中使用点到点的距离的平方。m个独立标准正太分布的平方和服从m自由度的卡方分布，选择阈值 使得点为内点的概率是  。就可以基于下式判断是否为内点

一般取=0.95,即点成为内点的概率为95%

 采样数量N：

多少次合适呢，尝试每个可能的样本计算量是没必要的，也可行，其实只要采样的次数足够多，以保证由s个点组成的随机样本至少有一次没有野值的概率为p，取为0.99. 一个点为内点的概率为 w，为外点的概率为 1-w。可以根据概率计算一下。

求解出：

                                                           

分析：

采样次数与内、外点的比例有关系，而与数据的量级无关系。
采样次数随着给最小点数s的增大而增大。

内点数量阈值T：

当我们发现内点的数量足够多的时候就可以提前结束采样。我们一般认为，在给定野值的假定比例后，如果一致集大小接近期望属于该数据集的内点数时迭代就停止。取N个数，。

自适应的决定采样次数

通常实际过程中实际的野值的概率是不知道的，对此情形，算法从的最坏估计开始，当发现更大的最大一致集时，就把原来的估计更新。

确定RANSAC样本方法的自适应算法

 直线例子：

namespace MVG {
	
int LineRANSAC::EstimateLineRansac ( const std::vector< Eigen::Vector2d >& points, const double p, const double sigma, double& A, double& B, double& C, std::vector& is_inlier )
{
	// Check input
	if(points.size() < 2 )
		return -1;
	
	const int n = points.size();
	int N = std::numeric_limits::max();
	const int T = 0.9*n; // TODO.
	int sample_count = 0;
	const double t2 = 3.84 * sigma * sigma;
	const int s = 2;
	
	cv::RNG rng(cv::getTickCount());
	int pre_inlier_cnt = 0;
	int final_inlier_cnt = 0;
	
	std::vector tmp_is_inlier(n);
	std::fill(tmp_is_inlier.begin(), tmp_is_inlier.end(), false);
	is_inlier.reserve(n);
		
	while(sample_count < N){
		
		// Step 1. Select s(2) points and estimate a line.
		int idx1 = rng.uniform(0, n);
		int idx2 = 0;
		while(true)
		{
			idx2 = rng.uniform(0, n);
			if(idx1 != idx2)
			{
				break;
			}
		}
		
		
		const Eigen::Vector2d& p1 = points.at(idx1);
		const Eigen::Vector2d& p2 = points.at(idx2);
		
		// Estimate
		double tmpA, tmpB, tmpC;
		findLine(p1, p2, tmpA, tmpB, tmpC);
		
		// Step 2. Find inlier points.
		int inlier_cnt = 0;
		for(size_t i = 0; i < points.size(); i++)
		{
			if(getDistance(points.at(i), tmpA, tmpB, tmpC) < t2)
			{
				tmp_is_inlier.at(i) = true;
				inlier_cnt ++;
			}
			else
			{
				tmp_is_inlier.at(i) = false;
			}
		}
		
		//std::cout << idx1 << " " << idx2 << std::endl;
		// Step 3. If we have enough pionts.
		if(inlier_cnt > T)
		{
			is_inlier = tmp_is_inlier;
			A = tmpA;
			B = tmpB;
			C = tmpC;
			final_inlier_cnt = inlier_cnt;
			// Break and refine the result.
			break;
		}
		
		// Update N.
		if(inlier_cnt > pre_inlier_cnt)
		{
			pre_inlier_cnt = inlier_cnt;
			
			// recompute N
			double w = (double)inlier_cnt / (double)n; // inlier prob./ratio.
			N = log(1.0-p) / log(1.0 - std::pow(w, s));
			
			// update the final model.
			is_inlier = tmp_is_inlier;
			A = tmpA;
			B = tmpB;
			C = tmpC;
			final_inlier_cnt = inlier_cnt;
		}
		sample_count ++;
	}
	
	// TODO Refine the result.
	
	return final_inlier_cnt;
}

void LineRANSAC::findLine ( const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2, double& A, double& B, double& C )
{
	A = pt1[1] - pt2[1];
	B = pt2[0] - pt1[0];
	C = pt1[0]*pt2[1] - pt2[0]*pt1[1];
}

double LineRANSAC::getDistance ( const Eigen::Vector2d& pt, const double A, const double B, const double C )
{
	return std::pow((A*pt[0] + B*pt[1] + C), 2.0) / (A*A + B*B);
}
	
	
} // namespace MVG

参考：

1.MVG（计算机多视几何）第二版

2.https://zhuanlan.zhihu.com/p/62175983