[基础知识点]RANSAC+鲁棒核函数

在V-SLAM中，我们首先构造3D-3D、3D-2D、2D-2D匹配，然后据此去估计相机的运动。完美估计需要完美的匹配，但实际的匹配中往往存在很多错误。如何消除或者降低错误匹配的影响呢？一种方法是选择那些正确的匹配进行估计（RANSAC），另一种是降低那些错误匹配的权重（鲁棒核函数）。

1. RANSAC 随机采样一致算法

1.1 相关定义

RANSAC算法的输入是一组观测数据（往往含有较大的噪声或无效点），一个用于解释观测数据的参数化模型以及一些可信的参数。

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

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

1.2 完整的RANSAC算法

STEP 1. 随机的从 $S$中选择$s$个点作为一个样本，估计出模型
STEP 2. 确定在模型距离阈值 $t$内的数据点集$S_i$ ，$S_i$ 即可为内点集
STEP 3. 如果 $S_i$的大小大于某个阈值$T$ ，用所有内点$S_i$ 重新估计一个更精确的模型
STEP 4. 如果 $S_i$ 的大小小于阈值 $T$ ，则重复1步骤
STEP 5. 经过$N$次试验，选择最大一致集 $S_i$ ，并用 $S_i$的所有点重新估计模型

图例：

1.2.1 估计模型——拟合2D直线

直线的一般方程：$Ax+By+C=0$
两边同除以$C$得： $A^{\ast }x+B^{\ast }y+1=0$


【注意】RANSAC里面有三个重要的参数需要确定，分别是$t$、 $T$ 、$N$ 。下面进行依次介绍。

1.2.2 内点阈值 $t$

根据模型，每个点可以计算出一个距离的平方$d^2$ ，利用阈值$t^2$ 就可以识别该点为内点还是外点。$d^2$要根据具体模型去选，例如，一般直线方程估计中采用点到线的距离的平方。m个独立标准正太分布的平方和服从m自由度的卡方分布，选择阈值$t$ 使得点为内点的概率是$\alpha$。基于下式判断是否为内点：

1.2.3 采样次数 $N$

1.2.4 内点数量阈值 $T$

1.3 核心代码

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<bool>& is_inlier )
{
	// Check input
	if(points.size() < 2 )
		return -1;
	
	const int n = points.size();
	int N = std::numeric_limits<int>::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<bool> 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);
}
	
	
} 

// create good data.
void createGoodData ( const double A, const double B, const double C,
                      const double minx, const double maxx,   
                      const int n,
					  const double sigma,
                      std::vector<Eigen::Vector2d>& points )
{
	// B != 0.
	points.clear();
	points.reserve(n+2);
	
	const double incx = (maxx - minx) / n;
	cv::RNG rng(cv::getTickCount());
	for(double x = minx; x < maxx; x += incx)
	{
		double y = (-C - A*x) / B;
		points.push_back(Eigen::Vector2d(x+rng.gaussian(sigma), y+rng.gaussian(sigma)));
	}
}

void createRandomData ( const double minx, const double miny,
                        const double maxx, const double maxy,
                        Eigen::Vector2d& rpt )
{
	cv::RNG rng(cv::getTickCount());
	rpt[0]  = rng.uniform(minx, maxx);
	rpt[1] = rng.uniform(miny, maxy);
}


int main ( int argc, char **argv )
{
	double A = 1.0, B = 1.0, C = 1.0;
	std::cout << "Line ground truth A B C: " << A << " " << B << " " << C <<"\n";
	
	std::vector<Eigen::Vector2d> points;
	createGoodData(A, B, C, -2.0, 2.0, 1000, 0.02, points);
	
	// add noise points
	for(int i = 0; i < 100; i ++)
	{
		Eigen::Vector2d pt;
		createRandomData(-2.0, -2.0, 2.0, 2.0, pt);
		points.push_back(pt);
	}
	// createGoodData(A, B, C, -2.0, 2.0, 1000, 0.02, points);
	
//	show points
// 	for(size_t i = 0; i < points.size(); i++)
// 	{
// 		const Eigen::Vector2d& pt = points.at(i);
// 		std::cout << std::fixed << std::setprecision(3) << pt[0] << " " << pt[1] << "\n";
// 	}
	
	std::vector<bool> is_inlier ;
	int nInlier = MVG::LineRANSAC::EstimateLineRansac(points, 0.99, 0.02, A, B, C, is_inlier);
	
	std::cout << "Line estimated A B C: " << A << " " << B << " " << C <<"\n";
	std::cout << "nInlier " << nInlier <<"\n\n";
    return 0;
}

1.4 实验结果

2. 鲁棒核函数

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