int mrpt::vision::pnp::p3p::solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2)
{
double mk0, mk1, mk2;
double norm;
mu0 = inv_fx * mu0 - cx_fx;
mv0 = inv_fy * mv0 - cy_fy;
norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
mu1 = inv_fx * mu1 - cx_fx;
mv1 = inv_fy * mv1 - cy_fy;
norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
mu2 = inv_fx * mu2 - cx_fx;
mv2 = inv_fy * mv2 - cy_fy;
norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
double distances[3];
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
// Calculate angles
double cosines[3];
cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
double lengths[4][3];
int n = solve_for_lengths(lengths, distances, cosines);
int nb_solutions = 0;
for(int i = 0; i < n; i++) {
double M_orig[3][3];
M_orig[0][0] = lengths[i][0] * mu0;
M_orig[0][1] = lengths[i][0] * mv0;
M_orig[0][2] = lengths[i][0] * mk0;
M_orig[1][0] = lengths[i][1] * mu1;
M_orig[1][1] = lengths[i][1] * mv1;
M_orig[1][2] = lengths[i][1] * mk1;
M_orig[2][0] = lengths[i][2] * mu2;
M_orig[2][1] = lengths[i][2] * mv2;
M_orig[2][2] = lengths[i][2] * mk2;
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
continue;
nb_solutions++;
}
return nb_solutions;
}
int p3p::solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3,
bool p4p)
{
double mk0, mk1, mk2;
double norm;
mu0 = inv_fx * mu0 - cx_fx;
mv0 = inv_fy * mv0 - cy_fy;
norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
mu1 = inv_fx * mu1 - cx_fx;
mv1 = inv_fy * mv1 - cy_fy;
norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
mu2 = inv_fx * mu2 - cx_fx;
mv2 = inv_fy * mv2 - cy_fy;
norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
mu3 = inv_fx * mu3 - cx_fx;
mv3 = inv_fy * mv3 - cy_fy;
double distances[3];
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
// Calculate angles
double cosines[3];
cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
double lengths[4][3] = {};
int n = solve_for_lengths(lengths, distances, cosines);
int nb_solutions = 0;
double reproj_errors[4];
for(int i = 0; i < n; i++) {
double M_orig[3][3];
M_orig[0][0] = lengths[i][0] * mu0;
M_orig[0][1] = lengths[i][0] * mv0;
M_orig[0][2] = lengths[i][0] * mk0;
M_orig[1][0] = lengths[i][1] * mu1;
M_orig[1][1] = lengths[i][1] * mv1;
M_orig[1][2] = lengths[i][1] * mk1;
M_orig[2][0] = lengths[i][2] * mu2;
M_orig[2][1] = lengths[i][2] * mv2;
M_orig[2][2] = lengths[i][2] * mk2;
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
continue;
if (p4p) {
double X3p = R[nb_solutions][0][0] * X3 + R[nb_solutions][0][1] * Y3 + R[nb_solutions][0][2] * Z3 + t[nb_solutions][0];
double Y3p = R[nb_solutions][1][0] * X3 + R[nb_solutions][1][1] * Y3 + R[nb_solutions][1][2] * Z3 + t[nb_solutions][1];
double Z3p = R[nb_solutions][2][0] * X3 + R[nb_solutions][2][1] * Y3 + R[nb_solutions][2][2] * Z3 + t[nb_solutions][2];
double mu3p = X3p / Z3p;
double mv3p = Y3p / Z3p;
reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
}
nb_solutions++;
}
if (p4p) {
//sort the solutions
for (int i = 1; i < nb_solutions; i++) {
for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
std::swap(reproj_errors[j], reproj_errors[j-1]);
std::swap(R[j], R[j-1]);
std::swap(t[j], t[j-1]);
}
}
}
return nb_solutions;
}
