1 #include "AbsoluteOrientationRANSAC.hh"
2 #include <Base/Math/Matrix3x3.hh>
3 #include <Base/Math/Matrix.hh>
4 #include <Base/Math/Vector.hh>
5 #include <Base/Geometry/Quaternion.hh>
6 #include <MathAlgo/SVD.hh>
11 # include <MathAlgo/GSL.hh>
17 AbsoluteOrientationRANSAC::AbsoluteOrientationRANSAC()
19 numSamples_(100), maxError_(0.01),
20 minRatio_(0.75), refine_(true), greedy_(false),
21 useWeightedTargetFunction_(false)
35 if (significance <= 0.0 || significance >= 1.0) {
36 BIASERR(
"Invalid significance level " << significance <<
37 " given (must be between 0.0 and 1.0)!");
42 BIASWARNONCE(
"No GSL support, using fixed significance level of 0.05!");
49 int AbsoluteOrientationRANSAC::
55 double &ds,
bool enforceScale,
56 std::vector<bool> &inliers, std::vector<double> &errors,
57 double &errorSum,
const std::vector<double> &w)
59 const unsigned int num = X.size();
60 const bool useCovs = (CovX.size() > 0 || CovY.size() > 0);
61 const bool useWeights = (w.size() > 0);
63 BIASASSERT(num >= sampleSize_);
64 BIASASSERT(X.size() == num && Y.size() == num);
65 BIASASSERT(!useCovs || (CovX.size() == num && CovY.size() == num));
66 BIASASSERT(!useWeights || w.size() == num);
68 if (useWeights && useCovs) {
69 BEXCEPTION(
"Weights and covariances should NOT be used at the same time "
70 "in robust absolute orientation estimation!");
75 std::vector<int> samples(0);
76 std::vector< Vector3<double> > sampleX(0);
77 std::vector< Vector3<double> > sampleY(0);
78 std::vector< Matrix3x3<double> > sampleCovX(0);
79 std::vector< Matrix3x3<double> > sampleCovY(0);
80 std::vector<double> samplew(0);
83 double actds, bestds = 0.0;
84 double actScore = 0.0, bestScore = 0.0;
85 double actErrorSum = 0.0;
87 int actInlierCount = -1, bestInlierCount = -1;
88 std::vector<bool> actInliers;
89 std::vector<double> actErrors;
95 for (
unsigned int k = 0; k < numSamples_ && !done; k++)
97 res = CreateRandomSample_(sampleSize_, X, Y, CovX, CovY, w,
98 samples, sampleX, sampleY,
99 sampleCovX, sampleCovY, samplew);
100 BIASASSERT(res == (
int)sampleSize_);
102 actdR = dR; actdt = dt; actds = ds;
103 res = solver_.Compute_(sampleX, sampleY, actdR, actdt, actds,
104 enforceScale, samplew);
106 actInlierCount = EvaluateInliers_(actInliers, actErrors, actErrorSum,
107 X, Y, CovX, CovY, actdR, actdt, actds, w);
108 if (useWeightedTargetFunction_)
111 for (
register unsigned int i = 0; i < num; i++) {
112 if (actInliers[i] && actErrors[i] <= maxError_) {
113 actScore += fabs(maxError_-actErrors[i])/maxError_;
117 actScore = (double)actInlierCount;
119 if (!init || actScore > bestScore) {
120 bestScore = actScore;
121 bestInlierCount = actInlierCount;
127 done = (greedy_ && bestInlierCount/(double)num >= minRatio_);
132 if (init && bestInlierCount >= (
int)sampleSize_)
138 EvaluateInliers_(actInliers, actErrors, actErrorSum,
139 X, Y, CovX, CovY, bestdR, bestdt, bestds, w);
143 samples.reserve(bestInlierCount);
144 sampleX.reserve(bestInlierCount);
145 sampleY.reserve(bestInlierCount);
149 sampleCovX.reserve(bestInlierCount);
150 sampleCovY.reserve(bestInlierCount);
154 samplew.reserve(bestInlierCount);
156 for (
register unsigned int i = 0; i < num; i++)
159 samples.push_back(i);
160 sampleX.push_back(X[i]);
161 sampleY.push_back(Y[i]);
163 sampleCovX.push_back(CovX[i]);
164 sampleCovY.push_back(CovY[i]);
167 samplew.push_back(w[i]);
171 BIASASSERT((
int)samples.size() == bestInlierCount);
172 dR = bestdR; dt = bestdt; ds = bestds;
173 res = solver_.Compute_(sampleX, sampleY, dR, dt, ds,
174 enforceScale, samplew);
176 return EvaluateInliers_(inliers, errors, errorSum,
177 X, Y, CovX, CovY, dR, dt, ds, w);
179 BIASWARN(
"Refinement of RANSAC solution failed!");
184 dR = bestdR; dt = bestdt; ds = bestds;
185 return EvaluateInliers_(inliers, errors, errorSum,
186 X, Y, CovX, CovY, dR, dt, ds, w);
188 BIASWARN(
"RANSAC failed, found no solution with enough inliers "
189 <<
"(found " << bestInlierCount <<
", needed min. "
190 << sampleSize_ <<
" inliers)!");
194 int AbsoluteOrientationRANSAC::
200 std::vector<double> &errors,
double &errorSum,
201 const std::vector<double> &w)
203 const unsigned int num = X.size();
204 const bool useCovs = (CovX.size() > 0 || CovY.size() > 0);
205 const bool useWeights = (w.size() > 0);
207 BIASASSERT(num >= sampleSize_);
208 BIASASSERT(X.size() == num && Y.size() == num);
209 BIASASSERT(!useCovs || (CovX.size() == num && CovY.size() == num));
210 BIASASSERT(!useWeights || w.size() == num);
212 if (useWeights && useCovs) {
213 BEXCEPTION(
"Weights and covariances should NOT be used at the same time "
214 "in robust absolute rotation estimation!");
219 std::vector<int> samples(0);
220 std::vector< Vector3<double> > sampleX(0);
221 std::vector< Vector3<double> > sampleY(0);
222 std::vector< Matrix3x3<double> > sampleCovX(0);
223 std::vector< Matrix3x3<double> > sampleCovY(0);
224 std::vector<double> samplew(0);
226 double actScore = 0.0, bestScore = 0.0;
227 double actErrorSum = 0.0;
229 int actInlierCount = -1, bestInlierCount = -1;
230 std::vector<bool> actInliers;
231 std::vector<double> actErrors;
237 for (
unsigned int k = 0; k < numSamples_ && !done; k++)
239 res = CreateRandomSample_(sampleSize_, X, Y, CovX, CovY, w,
240 samples, sampleX, sampleY,
241 sampleCovX, sampleCovY, samplew);
242 BIASASSERT(res == (
int)sampleSize_);
247 actInlierCount = EvaluateInliers_(actInliers, actErrors, actErrorSum,
248 X, Y, CovX, CovY, actdR, w);
249 if (useWeightedTargetFunction_)
252 for (
register unsigned int i = 0; i < num; i++) {
253 if (actInliers[i] && actErrors[i] <= maxError_) {
254 actScore += fabs(maxError_-actErrors[i])/maxError_;
258 actScore = (double)actInlierCount;
260 if (!init || actScore > bestScore) {
261 bestScore = actScore;
262 bestInlierCount = actInlierCount;
266 done = (greedy_ && bestInlierCount/(double)num >= minRatio_);
272 if (init && bestInlierCount >= (
int)sampleSize_)
277 EvaluateInliers_(actInliers, actErrors, actErrorSum,
278 X, Y, CovX, CovY, bestdR, w);
282 samples.reserve(bestInlierCount);
283 sampleX.reserve(bestInlierCount);
284 sampleY.reserve(bestInlierCount);
288 sampleCovX.reserve(bestInlierCount);
289 sampleCovY.reserve(bestInlierCount);
293 samplew.reserve(bestInlierCount);
295 for (
register unsigned int i = 0; i < num; i++)
298 samples.push_back(i);
299 sampleX.push_back(X[i]);
300 sampleY.push_back(Y[i]);
302 sampleCovX.push_back(CovX[i]);
303 sampleCovY.push_back(CovY[i]);
306 samplew.push_back(w[i]);
310 BIASASSERT((
int)samples.size() == bestInlierCount);
314 return EvaluateInliers_(inliers, errors, errorSum,
315 X, Y, CovX, CovY, dR, w);
317 BIASWARN(
"Refinement of RANSAC solution failed!");
323 return EvaluateInliers_(inliers, errors, errorSum,
324 X, Y, CovX, CovY, dR, w);
326 BIASWARN(
"RANSAC failed, found no solution with enough inliers "
327 <<
"(found " << bestInlierCount <<
", needed min. "
328 << sampleSize_ <<
" inliers)!");
337 const double &ds,
const std::vector<bool> &inliers,
338 const std::vector<double> &w)
340 const unsigned int num = X.size();
341 #ifdef BIASASSERT_ISACTIVE
342 const bool useWeights = (w.size() > 0);
344 const bool useInliers = (inliers.size() > 0);
346 BIASASSERT(X.size() == num && Y.size() == num);
347 BIASASSERT(!useInliers || inliers.size() == num);
348 BIASASSERT(!useWeights || w.size() == num);
352 double errorSum = 0.0;
354 for (
register unsigned int i = 0; i < num; i++) {
356 errorSum += fabs(errors[i]);
372 const double &ds,
const std::vector<bool> &inliers)
374 const unsigned int num = X.size();
375 const bool useInliers = (inliers.size() > 0);
377 BIASASSERT(X.size() == num && Y.size() == num);
378 BIASASSERT(CovX.size() == num && CovY.size() == num);
379 BIASASSERT(!useInliers || inliers.size() == num);
383 double errorSum = 0.0;
385 for (
register unsigned int i = 0; i < num; i++) {
387 errorSum += fabs(errors[i]);
401 const std::vector<double> &w)
403 const unsigned int num = X.size();
404 #ifdef BIASASSERT_ISACTIVE
405 const bool useWeights = (w.size() > 0);
407 const bool useInliers = (inliers.size() > 0);
409 BIASASSERT(X.size() == num && Y.size() == num);
410 BIASASSERT(!useInliers || inliers.size() == num);
411 BIASASSERT(!useWeights || w.size() == num);
415 double errorSum = 0.0;
417 for (
register unsigned int i = 0; i < num; i++) {
419 errorSum += fabs(errors[i]);
436 const unsigned int num = X.size();
437 const bool useInliers = (inliers.size() > 0);
439 BIASASSERT(X.size() == num && Y.size() == num);
440 BIASASSERT(CovX.size() == num && CovY.size() == num);
441 BIASASSERT(!useInliers || inliers.size() == num);
445 double errorSum = 0.0;
447 for (
register unsigned int i = 0; i < num; i++) {
449 errorSum += fabs(errors[i]);
458 int AbsoluteOrientationRANSAC::
459 CreateRandomSample_(
unsigned int samplesize,
464 const std::vector<double> &w,
465 std::vector<int> &samples,
470 std::vector<double> &samplew)
472 const unsigned int num = X.size();
473 const bool useCovs = (CovX.size() > 0 || CovY.size() > 0);
474 const bool useWeights = (w.size() > 0);
476 BIASASSERT(X.size() == num && Y.size() == num);
477 BIASASSERT(!useCovs || (CovX.size() == num && CovY.size() == num));
478 BIASASSERT(!useWeights || w.size() == num);
479 BIASASSERT(num >= samplesize);
484 samples.reserve(samplesize);
485 sampleX.reserve(samplesize);
486 sampleY.reserve(samplesize);
490 sampleCovX.reserve(samplesize);
491 sampleCovY.reserve(samplesize);
495 samplew.reserve(samplesize);
498 std::vector<bool> selected(num,
false);
499 for (
register unsigned int i = 0; i < samplesize; i++) {
501 for (
register unsigned int j = 0; selected[r] && j < num; j++) {
505 samples.push_back(r);
506 sampleX.push_back(X[r]);
507 sampleY.push_back(Y[r]);
509 sampleCovX.push_back(CovX[r]);
510 sampleCovY.push_back(CovY[r]);
513 samplew.push_back(w[r]);
516 return samples.size();
519 int AbsoluteOrientationRANSAC::
520 EvaluateInliers_(std::vector<bool> &inliers,
521 std::vector<double> &errors,
double &errorSum,
527 const std::vector<double> &w)
529 const unsigned int num = X.size();
530 const bool useCovs = (CovX.size() > 0 || CovY.size() > 0);
532 const bool useWeights = (w.size() > 0);
534 BIASASSERT(X.size() == num && Y.size() == num);
535 BIASASSERT(!useCovs || (CovX.size() == num && CovY.size() == num));
536 BIASASSERT(!useWeights || w.size() == num);
538 if (useWeights && useCovs) {
539 BIASERR(
"Weights and covariances should NOT be used at the same time!");
556 for (
register unsigned int i = 0; i < num; i++) {
557 error = fabs(errors[i]);
558 if (error > maxError_) {
569 int AbsoluteOrientationRANSAC::
570 EvaluateInliers_(std::vector<bool> &inliers,
571 std::vector<double> &errors,
double &errorSum,
576 const RMatrix &dR,
const std::vector<double> &w)
578 const unsigned int num = X.size();
579 const bool useCovs = (CovX.size() > 0 || CovY.size() > 0);
581 const bool useWeights = (w.size() > 0);
583 BIASASSERT(X.size() == num && Y.size() == num);
584 BIASASSERT(!useCovs || (CovX.size() == num && CovY.size() == num));
585 BIASASSERT(!useWeights || w.size() == num);
587 if (useWeights && useCovs) {
588 BIASERR(
"Weights and covariances should NOT be used at the same time!");
605 for (
register unsigned int i = 0; i < num; i++) {
606 error = fabs(errors[i]);
607 if (error > maxError_) {
Computes similarity transformation between 3D point sets.
~AbsoluteOrientationRANSAC()
int GetUniformDistributedInt(const int min, const int max)
get uniform distributed random variable including min/max
BIASMathAlgo_EXPORT double InvChiSquareCulmProbFun(double x, int deg_freedom)
double GetResidualErrors(std::vector< double > &errors, const std::vector< Vector3< double > > &X, const std::vector< Vector3< double > > &Y, const RMatrix &dR, const Vector3< double > &dt, const double &ds=1.0, const std::vector< double > &w=std::vector< double >(0))
Computes residual errors of 3D point sets X and Y with respect to given rotation dR, translation dt and isometric scale ds.
double GetResidualErrors(std::vector< double > &errors, const std::vector< Vector3< double > > &X, const std::vector< Vector3< double > > &Y, const RMatrix &dR, const Vector3< double > &dt, const double &ds=1.0, const std::vector< bool > &inliers=std::vector< bool >(0), const std::vector< double > &w=std::vector< double >(0))
Computes residual errors for inliers of 3D point sets X and Y with respect to given rotation dR...
void Reset()
calls srand() with a seed generated from system call time, also initializes some other variables ...
void SetSignificance(double significance)
Compute max.
int ComputeRotation(const std::vector< Vector3< double > > &X, const std::vector< Vector3< double > > &Y, RMatrix &dR, const std::vector< double > &w=std::vector< double >(0))
Computes rotation dR between corresponding 3D ray sets X and Y using the currently set algorithm...