cvar_core/OptimizationRANSAC.cpp at master · daisaru11/cvar_core · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
//
//  OptimizationRANSAC.cpp
//  cvar_core
//
//  Created by Daichi Sakai on 2013/01/13.
//  Copyright (c) 2013 Daichi Sakai. All rights reserved.
//


#include "OptimizationRANSAC.h"

OptimizationRANSAC::OptimizationRANSAC(const int num_model_points)
:num_model_points(num_model_points)
{
	// do nothing
}

OptimizationRANSAC::~OptimizationRANSAC()
{
	// do nothing
}

bool OptimizationRANSAC::getSubset(const cv::Mat& m1, const cv::Mat& m2, std::vector<int>& subset_idx)
{
	subset_idx.clear();
	int count = m1.rows;
	const int max_attempts = 300;
	int i, iters;
	for(i=0, iters=0; i<num_model_points && iters<max_attempts; )
	{
		int idx_i = rand() % count;
		int j;
		iters++;
		for (j=0; j<i; j++)
			if( idx_i == subset_idx[j] )
				break;
		if( j < i )
			continue;
		subset_idx.push_back(idx_i);
		i++;
	}

	return i == num_model_points && iters < max_attempts;
}

int RANSACUpdateNumIters( double p, double ep,
                        int model_points, int max_iters )
{
	// avoid inf's & nan's
	double num = MAX(1. - p, DBL_MIN);
	double denom = 1. - pow(1. - ep,model_points);
	if( denom < DBL_MIN )
		return 0;

	num = log(num);
	denom = log(denom);

	return denom >= 0 || -num >= max_iters*(-denom) ?
		max_iters : round(num/denom);
}

bool OptimizationRANSAC::run(const cv::Mat& m1, const cv::Mat& m2,
			cv::Mat& model, cv::Mat& mask,
			const double reproj_threshold, const double confidence,
			const int max_iters )
{
	bool result = false;
	const int count = m1.rows;
	int good_count_max = 0;
	std::vector<int> subset_idx;

	cv::Mat model_tmp;
	cv::Mat err(count, 1, CV_64F);
	cv::Mat tmask(count, 1, CV_8UC1);
	mask.create(count, 1, CV_8UC1);

	int niters = max_iters;
	for( int iter = 0; iter < niters; iter++ )
	{
		int good_count;
		if( count > num_model_points )
		{
			subset_idx.clear();
			bool found = getSubset( m1, m2, subset_idx );
			if( !found )
			{
				if( iter == 0 )
					return false;
				break;
			}
		}

		fitSubset(m1, m2, subset_idx, model_tmp);

		good_count = findInliers( m1, m2, model_tmp, err, tmask, reproj_threshold );
		if (good_count > good_count_max && good_count > num_model_points)
		{
			std::swap(tmask, mask);
			model_tmp.copyTo(model);
			good_count_max = good_count;
			niters = RANSACUpdateNumIters( confidence,
				(double)(count - good_count)/count, num_model_points, niters );
		}
	}

	if (good_count_max>0)
	{
		result = true;
	}

	return result;
}


int OptimizationRANSAC::findInliers( const cv::Mat& m1, const cv::Mat& m2,
		const cv::Mat& model, cv::Mat& err,
		cv::Mat& mask, double threshold )
{
	const int count = err.rows;
	int good_count = 0;
	const double* _err = reinterpret_cast<double *>(err.data);
	uchar* _mask = (uchar *)mask.data;

    threshold *= threshold;
	computeReprojError( m1, m2, model, err );
	for(int i = 0; i < count; i++ )
	{
		//std::cout << _err[i] << ", ";
		good_count += _mask[i] = _err[i] <= threshold;
	}
	//std::cout << std::endl;
	return good_count;
}