Example for similarity transformation estimation between two coordinate frames from corresponding 3D points., AbsoluteOrientationRANSAC AbsoluteOrientation estimates the relationship between two coordinate systems using pairs of measurements of the coordinates of a number of 3D points in both systems. A SVD-based solution to the least-squares problem for at least four points based on Horn's approach is used.
#include <Base/Math/Random.hh>
#include <Geometry/AbsoluteOrientation.hh>
#include <Geometry/AbsoluteOrientationRANSAC.hh>
#include <Utils/Param.hh>
#include <iostream>
#include <string>
#include <vector>
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[])
{
int* numParam = params.
AddParamInt(
"num",
"Number of 3D points", 10, 3,
std::numeric_limits<int>::max(), 'n', 0);
double* rangeParam = params.
AddParamDouble(
"range",
"Max. distance of 3D points "
"(in m)", 10.0, 1.e-8, 1e16, 'r', 0);
double* errorParam = params.
AddParamDouble(
"error",
"Max. error on 3D points "
"(in percent)", 0.01, 0.0, 1.0, 'e', 0);
bool* helpParam = params.
AddParamBool(
"help",
"Show help about parameters",
false, 'h', 0);
std::vector<std::string> methods;
std::vector<int> methodIDs;
methods.push_back("HORN");
methods.push_back("KABSCH");
int* methodParam = params.
AddParamEnum(
"method",
"Rotation estimation algorithm "
"(HORN, KABSCH)", methods, methodIDs[0],
&methodIDs, 'm', 0);
if (*helpParam) {
return 0;
}
unsigned int num = *numParam;
double range = *rangeParam;
double noise = *errorParam;
std::vector< BIAS::Vector3<double> > X;
std::vector< BIAS::Vector3<double> > Y;
cout << "- Compute absolute orientation from " << num << " 3D points";
if (noise > 0) cout << " with noise of " << 100.0*noise << "% using ";
cout << "Horn's method";
cout << "Kabsch algorithm";
else
cout << "undefined method";
cout << " for rotation estimation" << endl;
for (unsigned int k = 0; k < 3; k++) {
}
cout << "- Rotation dR = " << dphi*180.0/M_PI << " degree around axis ("
<< dr[0] << ", " << dr[1] << ", " << dr[2] << ")" << endl;
cout << "- Translation dt = (" << dt[0] << ", " << dt[1] << ", "
<< dt[2] << ")" << endl;
cout << "- Scale ds = " << ds << endl;
X.reserve(num);
Y.reserve(num);
for (unsigned int i = 0; i < num; i++) {
for (unsigned int k = 0; k < 3; k++) {
}
y = ds*dR*x + dt;
if (noise > 0) {
for (unsigned int k = 0; k < 3; k++) {
}
}
X.push_back(x);
Y.push_back(y);
cout << " [" << i+1 << "] X = (" << x[0] << ", " << x[1] << ", "
<< x[2] << "), Y = (" << y[0] << ", " << y[1] << ", " << y[2] << ")";
if (noise > 0) {
cout <<
", errors = " << errx.
NormL2() <<
" and " << erry.
NormL2();
}
cout << endl;
}
double estphi, ests;
int res = estimator.
Compute(X, Y, estR, estt, ests);
if (res != 0) {
cout << "[ERROR] Estimation failed!" << endl << endl;
return -1;
}
double errphi, error;
std::vector<double> errors;
cout << "- Estimated rotation dR = " << estphi*180.0/M_PI
<< " degree around axis (" << estr[0] << ", " << estr[1] << ", "
<< estr[2] << ")" << ", error = " << errphi*180.0/M_PI
<< " degree (" << 100.0*fabs(errphi-dphi)/dphi << "%)" << endl;
cout << "- Estimated translation dt = (" << estt[0] << ", " << estt[1]
<< ", " << estt[2] << "), error = " << (estt-dt).NormL2() << " ("
<< 100.0*(estt-dt).NormL2()/dt.
NormL2() <<
"%)" << endl;
cout << "- Estimated scale ds = " << ests << ", error = "
<< fabs(ests-ds) << " (" << 100.0*fabs(ests-ds)/ds << "%)" << endl;
cout << "- Average residual error of solution = "
<< sqrt(error/(double)num) << endl << endl;
return 0;
}