potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-30 07:43:03 +04:00
Code Packages Releases Wiki Activity

Merge branch 4.x

Browse Source
This commit is contained in:
Alexander Smorkalov
2023-09-11 13:22:22 +03:00
parent a6748df587 ee867ead1d
commit fdab565711
148 changed files with 2384 additions and 1876 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/3d/src/ap3p.cpp
+10 -62
View File
@@ -1,5 +1,6 @@
#include "precomp.hpp"
#include "ap3p.h"
#include "polynom_solver.h"
#include <cmath>
#include <complex>
@@ -7,67 +8,11 @@
static inline double cbrt(double x) { return (double)cv::cubeRoot((float)x); };
#endif
namespace cv {
static
void solveQuartic(const double *factors, double *realRoots)
{
const double &a4 = factors[0];
const double &a3 = factors[1];
const double &a2 = factors[2];
const double &a1 = factors[3];
const double &a0 = factors[4];
double a4_2 = a4 * a4;
double a3_2 = a3 * a3;
double a4_3 = a4_2 * a4;
double a2a4 = a2 * a4;
double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2);
double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3);
double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4));
double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3
double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2
double t; // *=2
std::complex<double> w;
if (q3 >= 0)
w = -std::sqrt(static_cast<std::complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
else
w = std::sqrt(static_cast<std::complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
if (w.imag() == 0.0) {
w.real(std::cbrt(w.real()));
t = 2.0 * (w.real() + p3 / w.real());
} else {
w = pow(w, 1.0 / 3);
t = 4.0 * w.real();
}
std::complex<double> sqrt_2m = sqrt(static_cast<std::complex<double> >(-2 * p4 / 3 + t));
double B_4A = -a3 / (4 * a4);
double complex1 = 4 * p4 / 3 + t;
#if defined(__clang__) && defined(__arm__) && (__clang_major__ == 3 || __clang_major__ == 4) && !defined(__ANDROID__)
// details: https://github.com/opencv/opencv/issues/11135
// details: https://github.com/opencv/opencv/issues/11056
std::complex<double> complex2 = 2 * q4;
complex2 = std::complex<double>(complex2.real() / sqrt_2m.real(), 0);
#else
std::complex<double> complex2 = 2 * q4 / sqrt_2m;
#endif
double sqrt_2m_rh = sqrt_2m.real() / 2;
double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2;
realRoots[0] = B_4A + sqrt_2m_rh + sqrt1;
realRoots[1] = B_4A + sqrt_2m_rh - sqrt1;
double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2;
realRoots[2] = B_4A - sqrt_2m_rh + sqrt2;
realRoots[3] = B_4A - sqrt_2m_rh - sqrt2;
}
static void polishQuarticRoots(const double *coeffs, double *roots) {
namespace {
void polishQuarticRoots(const double *coeffs, double *roots, int nb_roots) {
const int iterations = 2;
for (int i = 0; i < iterations; ++i) {
for (int j = 0; j < 4; ++j) {
for (int j = 0; j < nb_roots; ++j) {
double error =
(((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] +
coeffs[4];
@@ -124,7 +69,9 @@ inline void mat_mult(const double a[3][3], const double b[3][3], double result[3
result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
}
}
namespace cv {
void ap3p::init_inverse_parameters() {
inv_fx = 1. / fx;
inv_fy = 1. / fy;
@@ -228,8 +175,9 @@ int ap3p::computePoses(const double featureVectors[3][4],
2 * (g6 * g7 - g1 * g2 - g3 * g4),
g7 * g7 - g2 * g2 - g4 * g4};
double s[4];
solveQuartic(coeffs, s);
polishQuarticRoots(coeffs, s);
int nb_roots = solve_deg4(coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4],
s[0], s[1], s[2], s[3]);
polishQuarticRoots(coeffs, s, nb_roots);
double temp[3];
vect_cross(k1, nl, temp);
@@ -255,7 +203,7 @@ int ap3p::computePoses(const double featureVectors[3][4],
double reproj_errors[4];
int nb_solutions = 0;
for (int i = 0; i < 4; ++i) {
for (int i = 0; i < nb_roots; ++i) {
double ctheta1p = s[i];
if (abs(ctheta1p) > 1)
continue;