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

目录

  • 1. RANSAC 随机采样一致算法
      • 1.1 相关定义
      • 1.2 完整的RANSAC算法
        • 1.2.1 估计模型——拟合2D直线
        • 1.2.2 内点阈值 t t t
        • 1.2.3 采样次数 N N N
        • 1.2.4 内点数量阈值 T T T
      • 1.3 核心代码
      • 1.4 实验结果
  • 2. 鲁棒核函数

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

1. RANSAC 随机采样一致算法

1.1 相关定义

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

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

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

1.2 完整的RANSAC算法

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

图例:

[基础知识点]RANSAC+鲁棒核函数_第1张图片

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

直线的一般方程: A x + B y + C = 0 Ax + By + C = 0 Ax+By+C=0
两边同除以 C C C得: A ∗ x + B ∗ y + 1 = 0 A^* x + B^*y+1 = 0 Ax+By+1=0
[基础知识点]RANSAC+鲁棒核函数_第2张图片
[基础知识点]RANSAC+鲁棒核函数_第3张图片
【注意】RANSAC里面有三个重要的参数需要确定,分别是 t t t T T T N N N 。下面进行依次介绍。

1.2.2 内点阈值 t t t

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

[基础知识点]RANSAC+鲁棒核函数_第4张图片
[基础知识点]RANSAC+鲁棒核函数_第5张图片

1.2.3 采样次数 N N N

[基础知识点]RANSAC+鲁棒核函数_第6张图片

1.2.4 内点数量阈值 T T 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 实验结果

[基础知识点]RANSAC+鲁棒核函数_第7张图片

2. 鲁棒核函数

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

[基础知识点]RANSAC+鲁棒核函数_第8张图片
[基础知识点]RANSAC+鲁棒核函数_第9张图片

你可能感兴趣的:(计算机视觉)