shen/Happy-Reconstruction
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add part of opencv

Browse Source
This commit is contained in:
Tang1705
2020-01-27 20:20:56 +08:00
parent 0c4ac1d8bb
commit a71fa47620
6518 changed files with 3122580 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

453
Lib/opencv/sources/modules/calib3d/src/ap3p.cpp Normal file
View File

@@ -0,0 +1,453 @@
#include "precomp.hpp"
#include "ap3p.h"
#include <cmath>
#include <complex>
#if defined (_MSC_VER) && (_MSC_VER <= 1700)
static inline double cbrt(double x) { return (double)cv::cubeRoot((float)x); };
#endif
using namespace std;
namespace {
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
complex<double> w;
if (q3 >= 0)
w = -sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
else
w = sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
if (w.imag() == 0.0) {
w.real(cbrt(w.real()));
t = 2.0 * (w.real() + p3 / w.real());
} else {
w = pow(w, 1.0 / 3);
t = 4.0 * w.real();
}
complex<double> sqrt_2m = sqrt(static_cast<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_minor__ == 4) && !defined(__ANDROID__)
// details: https://github.com/opencv/opencv/issues/11135
// details: https://github.com/opencv/opencv/issues/11056
complex<double> complex2 = 2 * q4;
complex2 = complex<double>(complex2.real() / sqrt_2m.real(), 0);
#else
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;
}
void polishQuarticRoots(const double *coeffs, double *roots) {
const int iterations = 2;
for (int i = 0; i < iterations; ++i) {
for (int j = 0; j < 4; ++j) {
double error =
(((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] +
coeffs[4];
double
derivative =
((4 * coeffs[0] * roots[j] + 3 * coeffs[1]) * roots[j] + 2 * coeffs[2]) * roots[j] + coeffs[3];
roots[j] -= error / derivative;
}
}
}
inline void vect_cross(const double *a, const double *b, double *result) {
result[0] = a[1] * b[2] - a[2] * b[1];
result[1] = -(a[0] * b[2] - a[2] * b[0]);
result[2] = a[0] * b[1] - a[1] * b[0];
}
inline double vect_dot(const double *a, const double *b) {
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
inline double vect_norm(const double *a) {
return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
}
inline void vect_scale(const double s, const double *a, double *result) {
result[0] = a[0] * s;
result[1] = a[1] * s;
result[2] = a[2] * s;
}
inline void vect_sub(const double *a, const double *b, double *result) {
result[0] = a[0] - b[0];
result[1] = a[1] - b[1];
result[2] = a[2] - b[2];
}
inline void vect_divide(const double *a, const double d, double *result) {
result[0] = a[0] / d;
result[1] = a[1] / d;
result[2] = a[2] / d;
}
inline void mat_mult(const double a[3][3], const double b[3][3], double result[3][3]) {
result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
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;
cx_fx = cx / fx;
cy_fy = cy / fy;
}
ap3p::ap3p(cv::Mat cameraMatrix) {
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_inverse_parameters();
}
ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
fx = _fx;
fy = _fy;
cx = _cx;
cy = _cy;
init_inverse_parameters();
}
// This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
// See https://arxiv.org/pdf/1701.08237.pdf
// featureVectors: The 3 bearing measurements (normalized) stored as column vectors
// worldPoints: The positions of the 3 feature points stored as column vectors
// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
int ap3p::computePoses(const double featureVectors[3][4],
const double worldPoints[3][4],
double solutionsR[4][3][3],
double solutionsT[4][3],
bool p4p) {
//world point vectors
double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
double w2[3] = {worldPoints[0][1], worldPoints[1][1], worldPoints[2][1]};
double w3[3] = {worldPoints[0][2], worldPoints[1][2], worldPoints[2][2]};
// k1
double u0[3];
vect_sub(w1, w2, u0);
double nu0 = vect_norm(u0);
double k1[3];
vect_divide(u0, nu0, k1);
// bi
double b1[3] = {featureVectors[0][0], featureVectors[1][0], featureVectors[2][0]};
double b2[3] = {featureVectors[0][1], featureVectors[1][1], featureVectors[2][1]};
double b3[3] = {featureVectors[0][2], featureVectors[1][2], featureVectors[2][2]};
// k3,tz
double k3[3];
vect_cross(b1, b2, k3);
double nk3 = vect_norm(k3);
vect_divide(k3, nk3, k3);
double tz[3];
vect_cross(b1, k3, tz);
// ui,vi
double v1[3];
vect_cross(b1, b3, v1);
double v2[3];
vect_cross(b2, b3, v2);
double u1[3];
vect_sub(w1, w3, u1);
// coefficients related terms
double u1k1 = vect_dot(u1, k1);
double k3b3 = vect_dot(k3, b3);
// f1i
double f11 = k3b3;
double f13 = vect_dot(k3, v1);
double f15 = -u1k1 * f11;
//delta
double nl[3];
vect_cross(u1, k1, nl);
double delta = vect_norm(nl);
vect_divide(nl, delta, nl);
f11 *= delta;
f13 *= delta;
// f2i
double u2k1 = u1k1 - nu0;
double f21 = vect_dot(tz, v2);
double f22 = nk3 * k3b3;
double f23 = vect_dot(k3, v2);
double f24 = u2k1 * f22;
double f25 = -u2k1 * f21;
f21 *= delta;
f22 *= delta;
f23 *= delta;
double g1 = f13 * f22;
double g2 = f13 * f25 - f15 * f23;
double g3 = f11 * f23 - f13 * f21;
double g4 = -f13 * f24;
double g5 = f11 * f22;
double g6 = f11 * f25 - f15 * f21;
double g7 = -f15 * f24;
double coeffs[5] = {g5 * g5 + g1 * g1 + g3 * g3,
2 * (g5 * g6 + g1 * g2 + g3 * g4),
g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3,
2 * (g6 * g7 - g1 * g2 - g3 * g4),
g7 * g7 - g2 * g2 - g4 * g4};
double s[4];
solveQuartic(coeffs, s);
polishQuarticRoots(coeffs, s);
double temp[3];
vect_cross(k1, nl, temp);
double Ck1nl[3][3] =
{{k1[0], nl[0], temp[0]},
{k1[1], nl[1], temp[1]},
{k1[2], nl[2], temp[2]}};
double Cb1k3tzT[3][3] =
{{b1[0], b1[1], b1[2]},
{k3[0], k3[1], k3[2]},
{tz[0], tz[1], tz[2]}};
double b3p[3];
vect_scale((delta / k3b3), b3, b3p);
double X3 = worldPoints[0][3];
double Y3 = worldPoints[1][3];
double Z3 = worldPoints[2][3];
double mu3 = featureVectors[0][3];
double mv3 = featureVectors[1][3];
double reproj_errors[4];
int nb_solutions = 0;
for (int i = 0; i < 4; ++i) {
double ctheta1p = s[i];
if (abs(ctheta1p) > 1)
continue;
double stheta1p = sqrt(1 - ctheta1p * ctheta1p);
stheta1p = (k3b3 > 0) ? stheta1p : -stheta1p;
double ctheta3 = g1 * ctheta1p + g2;
double stheta3 = g3 * ctheta1p + g4;
double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7);
ctheta3 *= ntheta3;
stheta3 *= ntheta3;
double C13[3][3] =
{{ctheta3, 0, -stheta3},
{stheta1p * stheta3, ctheta1p, stheta1p * ctheta3},
{ctheta1p * stheta3, -stheta1p, ctheta1p * ctheta3}};
double temp_matrix[3][3];
double R[3][3];
mat_mult(Ck1nl, C13, temp_matrix);
mat_mult(temp_matrix, Cb1k3tzT, R);
// R' * p3
double rp3[3] =
{w3[0] * R[0][0] + w3[1] * R[1][0] + w3[2] * R[2][0],
w3[0] * R[0][1] + w3[1] * R[1][1] + w3[2] * R[2][1],
w3[0] * R[0][2] + w3[1] * R[1][2] + w3[2] * R[2][2]};
double pxstheta1p[3];
vect_scale(stheta1p, b3p, pxstheta1p);
vect_sub(pxstheta1p, rp3, solutionsT[nb_solutions]);
solutionsR[nb_solutions][0][0] = R[0][0];
solutionsR[nb_solutions][1][0] = R[0][1];
solutionsR[nb_solutions][2][0] = R[0][2];
solutionsR[nb_solutions][0][1] = R[1][0];
solutionsR[nb_solutions][1][1] = R[1][1];
solutionsR[nb_solutions][2][1] = R[1][2];
solutionsR[nb_solutions][0][2] = R[2][0];
solutionsR[nb_solutions][1][2] = R[2][1];
solutionsR[nb_solutions][2][2] = R[2][2];
if (p4p) {
double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0];
double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1];
double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[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++;
}
//sort the solutions
if (p4p) {
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(solutionsR[j], solutionsR[j-1]);
std::swap(solutionsT[j], solutionsT[j-1]);
}
}
}
return nb_solutions;
}
bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints) {
CV_INSTRUMENT_REGION();
double rotation_matrix[3][3] = {}, translation[3] = {};
std::vector<double> points;
if (opoints.depth() == ipoints.depth()) {
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
} else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
bool result = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13],points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
return result;
}
int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
CV_INSTRUMENT_REGION();
double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
std::vector<double> points;
if (opoints.depth() == ipoints.depth()) {
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
} else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19],
p4p);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool
ap3p::solve(double R[3][3], double t[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) {
double Rs[4][3][3] = {}, ts[4][3] = {};
const bool p4p = true;
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
if (n == 0)
return false;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++)
R[i][j] = Rs[0][i][j];
t[i] = ts[0][i];
}
return true;
}
int ap3p::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 mk3 = 1; //not used
double featureVectors[3][4] = {{mu0, mu1, mu2, mu3},
{mv0, mv1, mv2, mv3},
{mk0, mk1, mk2, mk3}};
double worldPoints[3][4] = {{X0, X1, X2, X3},
{Y0, Y1, Y2, Y3},
{Z0, Z1, Z2, Z3}};
return computePoses(featureVectors, worldPoints, R, t, p4p);
}
}

75
Lib/opencv/sources/modules/calib3d/src/ap3p.h Normal file
View File

@@ -0,0 +1,75 @@
#ifndef P3P_P3P_H
#define P3P_P3P_H
#include <opencv2/core.hpp>
namespace cv {
class ap3p {
private:
template<typename T>
void init_camera_parameters(const cv::Mat &cameraMatrix) {
cx = cameraMatrix.at<T>(0, 2);
cy = cameraMatrix.at<T>(1, 2);
fx = cameraMatrix.at<T>(0, 0);
fy = cameraMatrix.at<T>(1, 1);
}
template<typename OpointType, typename IpointType>
void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
points.clear();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*4); //resize vector to fit for p4p case
for (int i = 0; i < npoints; i++) {
points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
points[i * 5 + 2] = opoints.at<OpointType>(i).x;
points[i * 5 + 3] = opoints.at<OpointType>(i).y;
points[i * 5 + 4] = opoints.at<OpointType>(i).z;
}
//Fill vectors with unused values for p3p case
for (int i = npoints; i < 4; i++) {
for (int j = 0; j < 5; j++) {
points[i * 5 + j] = 0;
}
}
}
void init_inverse_parameters();
double fx, fy, cx, cy;
double inv_fx, inv_fy, cx_fx, cy_fy;
public:
ap3p() : fx(0), fy(0), cx(0), cy(0), inv_fx(0), inv_fy(0), cx_fx(0), cy_fy(0) {}
ap3p(double fx, double fy, double cx, double cy);
ap3p(cv::Mat cameraMatrix);
bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints);
int 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);
bool solve(double R[3][3], double t[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);
// This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
// See https://arxiv.org/pdf/1701.08237.pdf
// featureVectors: 3 bearing measurements (normalized) stored as column vectors
// worldPoints: Positions of the 3 feature points stored as column vectors
// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3],
double solutionsT[4][3], bool p4p);
};
}
#endif //P3P_P3P_H

425
Lib/opencv/sources/modules/calib3d/src/calib3d_c_api.h Normal file
View File

@@ -0,0 +1,425 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef OPENCV_CALIB3D_C_API_H
#define OPENCV_CALIB3D_C_API_H
#include "opencv2/core/core_c.h"
#include "opencv2/calib3d/calib3d_c.h"
#ifdef __cplusplus
extern "C" {
#endif
/** @addtogroup calib3d_c
@{
*/
/****************************************************************************************\
* Camera Calibration, Pose Estimation and Stereo *
\****************************************************************************************/
typedef struct CvPOSITObject CvPOSITObject;
/* Allocates and initializes CvPOSITObject structure before doing cvPOSIT */
CvPOSITObject* cvCreatePOSITObject( CvPoint3D32f* points, int point_count );
/* Runs POSIT (POSe from ITeration) algorithm for determining 3d position of
an object given its model and projection in a weak-perspective case */
void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points,
double focal_length, CvTermCriteria criteria,
float* rotation_matrix, float* translation_vector);
/* Releases CvPOSITObject structure */
void cvReleasePOSITObject( CvPOSITObject** posit_object );
/* updates the number of RANSAC iterations */
int cvRANSACUpdateNumIters( double p, double err_prob,
int model_points, int max_iters );
void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst );
/* Calculates fundamental matrix given a set of corresponding points */
/*#define CV_FM_7POINT 1
#define CV_FM_8POINT 2
#define CV_LMEDS 4
#define CV_RANSAC 8
#define CV_FM_LMEDS_ONLY CV_LMEDS
#define CV_FM_RANSAC_ONLY CV_RANSAC
#define CV_FM_LMEDS CV_LMEDS
#define CV_FM_RANSAC CV_RANSAC*/
int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
CvMat* fundamental_matrix,
int method CV_DEFAULT(CV_FM_RANSAC),
double param1 CV_DEFAULT(3.), double param2 CV_DEFAULT(0.99),
CvMat* status CV_DEFAULT(NULL) );
/* For each input point on one of images
computes parameters of the corresponding
epipolar line on the other image */
void cvComputeCorrespondEpilines( const CvMat* points,
int which_image,
const CvMat* fundamental_matrix,
CvMat* correspondent_lines );
/* Triangulation functions */
void cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2,
CvMat* projPoints1, CvMat* projPoints2,
CvMat* points4D);
void cvCorrectMatches(CvMat* F, CvMat* points1, CvMat* points2,
CvMat* new_points1, CvMat* new_points2);
/* Computes the optimal new camera matrix according to the free scaling parameter alpha:
alpha=0 - only valid pixels will be retained in the undistorted image
alpha=1 - all the source image pixels will be retained in the undistorted image
*/
void cvGetOptimalNewCameraMatrix( const CvMat* camera_matrix,
const CvMat* dist_coeffs,
CvSize image_size, double alpha,
CvMat* new_camera_matrix,
CvSize new_imag_size CV_DEFAULT(cvSize(0,0)),
CvRect* valid_pixel_ROI CV_DEFAULT(0),
int center_principal_point CV_DEFAULT(0));
/* Converts rotation vector to rotation matrix or vice versa */
int cvRodrigues2( const CvMat* src, CvMat* dst,
CvMat* jacobian CV_DEFAULT(0) );
/* Finds perspective transformation between the object plane and image (view) plane */
int cvFindHomography( const CvMat* src_points,
const CvMat* dst_points,
CvMat* homography,
int method CV_DEFAULT(0),
double ransacReprojThreshold CV_DEFAULT(3),
CvMat* mask CV_DEFAULT(0),
int maxIters CV_DEFAULT(2000),
double confidence CV_DEFAULT(0.995));
/* Computes RQ decomposition for 3x3 matrices */
void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx CV_DEFAULT(NULL),
CvMat *matrixQy CV_DEFAULT(NULL),
CvMat *matrixQz CV_DEFAULT(NULL),
CvPoint3D64f *eulerAngles CV_DEFAULT(NULL));
/* Computes projection matrix decomposition */
void cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX CV_DEFAULT(NULL),
CvMat *rotMatrY CV_DEFAULT(NULL),
CvMat *rotMatrZ CV_DEFAULT(NULL),
CvPoint3D64f *eulerAngles CV_DEFAULT(NULL));
/* Computes d(AB)/dA and d(AB)/dB */
void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB );
/* Computes r3 = rodrigues(rodrigues(r2)*rodrigues(r1)),
t3 = rodrigues(r2)*t1 + t2 and the respective derivatives */
void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
const CvMat* _rvec2, const CvMat* _tvec2,
CvMat* _rvec3, CvMat* _tvec3,
CvMat* dr3dr1 CV_DEFAULT(0), CvMat* dr3dt1 CV_DEFAULT(0),
CvMat* dr3dr2 CV_DEFAULT(0), CvMat* dr3dt2 CV_DEFAULT(0),
CvMat* dt3dr1 CV_DEFAULT(0), CvMat* dt3dt1 CV_DEFAULT(0),
CvMat* dt3dr2 CV_DEFAULT(0), CvMat* dt3dt2 CV_DEFAULT(0) );
/* Projects object points to the view plane using
the specified extrinsic and intrinsic camera parameters */
void cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
const CvMat* translation_vector, const CvMat* camera_matrix,
const CvMat* distortion_coeffs, CvMat* image_points,
CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
CvMat* dpddist CV_DEFAULT(NULL),
double aspect_ratio CV_DEFAULT(0));
/* Finds extrinsic camera parameters from
a few known corresponding point pairs and intrinsic parameters */
void cvFindExtrinsicCameraParams2( const CvMat* object_points,
const CvMat* image_points,
const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
CvMat* rotation_vector,
CvMat* translation_vector,
int use_extrinsic_guess CV_DEFAULT(0) );
/* Computes initial estimate of the intrinsic camera parameters
in case of planar calibration target (e.g. chessboard) */
void cvInitIntrinsicParams2D( const CvMat* object_points,
const CvMat* image_points,
const CvMat* npoints, CvSize image_size,
CvMat* camera_matrix,
double aspect_ratio CV_DEFAULT(1.) );
// Performs a fast check if a chessboard is in the input image. This is a workaround to
// a problem of cvFindChessboardCorners being slow on images with no chessboard
// - src: input image
// - size: chessboard size
// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
// 0 if there is no chessboard, -1 in case of error
int cvCheckChessboard(IplImage* src, CvSize size);
/* Detects corners on a chessboard calibration pattern */
/*int cvFindChessboardCorners( const void* image, CvSize pattern_size,
CvPoint2D32f* corners,
int* corner_count CV_DEFAULT(NULL),
int flags CV_DEFAULT(CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE) );*/
/* Draws individual chessboard corners or the whole chessboard detected */
/*void cvDrawChessboardCorners( CvArr* image, CvSize pattern_size,
CvPoint2D32f* corners,
int count, int pattern_was_found );*/
/*#define CV_CALIB_USE_INTRINSIC_GUESS 1
#define CV_CALIB_FIX_ASPECT_RATIO 2
#define CV_CALIB_FIX_PRINCIPAL_POINT 4
#define CV_CALIB_ZERO_TANGENT_DIST 8
#define CV_CALIB_FIX_FOCAL_LENGTH 16
#define CV_CALIB_FIX_K1 32
#define CV_CALIB_FIX_K2 64
#define CV_CALIB_FIX_K3 128
#define CV_CALIB_FIX_K4 2048
#define CV_CALIB_FIX_K5 4096
#define CV_CALIB_FIX_K6 8192
#define CV_CALIB_RATIONAL_MODEL 16384
#define CV_CALIB_THIN_PRISM_MODEL 32768
#define CV_CALIB_FIX_S1_S2_S3_S4 65536
#define CV_CALIB_TILTED_MODEL 262144
#define CV_CALIB_FIX_TAUX_TAUY 524288
#define CV_CALIB_FIX_TANGENT_DIST 2097152
#define CV_CALIB_NINTRINSIC 18*/
/* Finds intrinsic and extrinsic camera parameters
from a few views of known calibration pattern */
double cvCalibrateCamera2( const CvMat* object_points,
const CvMat* image_points,
const CvMat* point_counts,
CvSize image_size,
CvMat* camera_matrix,
CvMat* distortion_coeffs,
CvMat* rotation_vectors CV_DEFAULT(NULL),
CvMat* translation_vectors CV_DEFAULT(NULL),
int flags CV_DEFAULT(0),
CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(
CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) );
/* Finds intrinsic and extrinsic camera parameters
from a few views of known calibration pattern */
double cvCalibrateCamera4( const CvMat* object_points,
const CvMat* image_points,
const CvMat* point_counts,
CvSize image_size,
int iFixedPoint,
CvMat* camera_matrix,
CvMat* distortion_coeffs,
CvMat* rotation_vectors CV_DEFAULT(NULL),
CvMat* translation_vectors CV_DEFAULT(NULL),
CvMat* newObjPoints CV_DEFAULT(NULL),
int flags CV_DEFAULT(0),
CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(
CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) );
/* Computes various useful characteristics of the camera from the data computed by
cvCalibrateCamera2 */
void cvCalibrationMatrixValues( const CvMat *camera_matrix,
CvSize image_size,
double aperture_width CV_DEFAULT(0),
double aperture_height CV_DEFAULT(0),
double *fovx CV_DEFAULT(NULL),
double *fovy CV_DEFAULT(NULL),
double *focal_length CV_DEFAULT(NULL),
CvPoint2D64f *principal_point CV_DEFAULT(NULL),
double *pixel_aspect_ratio CV_DEFAULT(NULL));
/*#define CV_CALIB_FIX_INTRINSIC 256
#define CV_CALIB_SAME_FOCAL_LENGTH 512*/
/* Computes the transformation from one camera coordinate system to another one
from a few correspondent views of the same calibration target. Optionally, calibrates
both cameras */
double cvStereoCalibrate( const CvMat* object_points, const CvMat* image_points1,
const CvMat* image_points2, const CvMat* npoints,
CvMat* camera_matrix1, CvMat* dist_coeffs1,
CvMat* camera_matrix2, CvMat* dist_coeffs2,
CvSize image_size, CvMat* R, CvMat* T,
CvMat* E CV_DEFAULT(0), CvMat* F CV_DEFAULT(0),
int flags CV_DEFAULT(CV_CALIB_FIX_INTRINSIC),
CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(
CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,1e-6)) );
#define CV_CALIB_ZERO_DISPARITY 1024
/* Computes 3D rotations (+ optional shift) for each camera coordinate system to make both
views parallel (=> to make all the epipolar lines horizontal or vertical) */
void cvStereoRectify( const CvMat* camera_matrix1, const CvMat* camera_matrix2,
const CvMat* dist_coeffs1, const CvMat* dist_coeffs2,
CvSize image_size, const CvMat* R, const CvMat* T,
CvMat* R1, CvMat* R2, CvMat* P1, CvMat* P2,
CvMat* Q CV_DEFAULT(0),
int flags CV_DEFAULT(CV_CALIB_ZERO_DISPARITY),
double alpha CV_DEFAULT(-1),
CvSize new_image_size CV_DEFAULT(cvSize(0,0)),
CvRect* valid_pix_ROI1 CV_DEFAULT(0),
CvRect* valid_pix_ROI2 CV_DEFAULT(0));
/* Computes rectification transformations for uncalibrated pair of images using a set
of point correspondences */
int cvStereoRectifyUncalibrated( const CvMat* points1, const CvMat* points2,
const CvMat* F, CvSize img_size,
CvMat* H1, CvMat* H2,
double threshold CV_DEFAULT(5));
/* stereo correspondence parameters and functions */
#define CV_STEREO_BM_NORMALIZED_RESPONSE 0
#define CV_STEREO_BM_XSOBEL 1
/* Block matching algorithm structure */
typedef struct CvStereoBMState
{
// pre-filtering (normalization of input images)
int preFilterType; // =CV_STEREO_BM_NORMALIZED_RESPONSE now
int preFilterSize; // averaging window size: ~5x5..21x21
int preFilterCap; // the output of pre-filtering is clipped by [-preFilterCap,preFilterCap]
// correspondence using Sum of Absolute Difference (SAD)
int SADWindowSize; // ~5x5..21x21
int minDisparity; // minimum disparity (can be negative)
int numberOfDisparities; // maximum disparity - minimum disparity (> 0)
// post-filtering
int textureThreshold; // the disparity is only computed for pixels
// with textured enough neighborhood
int uniquenessRatio; // accept the computed disparity d* only if
// SAD(d) >= SAD(d*)*(1 + uniquenessRatio/100.)
// for any d != d*+/-1 within the search range.
int speckleWindowSize; // disparity variation window
int speckleRange; // acceptable range of variation in window
int trySmallerWindows; // if 1, the results may be more accurate,
// at the expense of slower processing
CvRect roi1, roi2;
int disp12MaxDiff;
// temporary buffers
CvMat* preFilteredImg0;
CvMat* preFilteredImg1;
CvMat* slidingSumBuf;
CvMat* cost;
CvMat* disp;
} CvStereoBMState;
#define CV_STEREO_BM_BASIC 0
#define CV_STEREO_BM_FISH_EYE 1
#define CV_STEREO_BM_NARROW 2
CvStereoBMState* cvCreateStereoBMState(int preset CV_DEFAULT(CV_STEREO_BM_BASIC),
int numberOfDisparities CV_DEFAULT(0));
void cvReleaseStereoBMState( CvStereoBMState** state );
void cvFindStereoCorrespondenceBM( const CvArr* left, const CvArr* right,
CvArr* disparity, CvStereoBMState* state );
CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity,
int numberOfDisparities, int SADWindowSize );
void cvValidateDisparity( CvArr* disparity, const CvArr* cost,
int minDisparity, int numberOfDisparities,
int disp12MaxDiff CV_DEFAULT(1) );
/* Reprojects the computed disparity image to the 3D space using the specified 4x4 matrix */
void cvReprojectImageTo3D( const CvArr* disparityImage,
CvArr* _3dImage, const CvMat* Q,
int handleMissingValues CV_DEFAULT(0) );
/** @brief Transforms the input image to compensate lens distortion
@see cv::undistort
*/
void cvUndistort2( const CvArr* src, CvArr* dst,
const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
const CvMat* new_camera_matrix CV_DEFAULT(0) );
/** @brief Computes transformation map from intrinsic camera parameters
that can used by cvRemap
*/
void cvInitUndistortMap( const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
CvArr* mapx, CvArr* mapy );
/** @brief Computes undistortion+rectification map for a head of stereo camera
@see cv::initUndistortRectifyMap
*/
void cvInitUndistortRectifyMap( const CvMat* camera_matrix,
const CvMat* dist_coeffs,
const CvMat *R, const CvMat* new_camera_matrix,
CvArr* mapx, CvArr* mapy );
/** @brief Computes the original (undistorted) feature coordinates
from the observed (distorted) coordinates
@see cv::undistortPoints
*/
void cvUndistortPoints( const CvMat* src, CvMat* dst,
const CvMat* camera_matrix,
const CvMat* dist_coeffs,
const CvMat* R CV_DEFAULT(0),
const CvMat* P CV_DEFAULT(0));
/** @} calib3d_c */
#ifdef __cplusplus
} // extern "C"
#endif
#endif /* OPENCV_CALIB3D_C_API_H */

2286
Lib/opencv/sources/modules/calib3d/src/calibinit.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

4242
Lib/opencv/sources/modules/calib3d/src/calibration.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

770
Lib/opencv/sources/modules/calib3d/src/calibration_handeye.cpp Normal file
View File

@@ -0,0 +1,770 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "precomp.hpp"
#include "opencv2/calib3d.hpp"
namespace cv {
static Mat homogeneousInverse(const Mat& T)
{
CV_Assert(T.rows == 4 && T.cols == 4);
Mat R = T(Rect(0, 0, 3, 3));
Mat t = T(Rect(3, 0, 1, 3));
Mat Rt = R.t();
Mat tinv = -Rt * t;
Mat Tinv = Mat::eye(4, 4, T.type());
Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
return Tinv;
}
// q = rot2quatMinimal(R)
//
// R - 3x3 rotation matrix, or 4x4 homogeneous matrix
// q - 3x1 unit quaternion <qx, qy, qz>
// q = sin(theta/2) * v
// theta - rotation angle
// v - unit rotation axis, |v| = 1
static Mat rot2quatMinimal(const Mat& R)
{
CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
double trace = m00 + m11 + m22;
double qx, qy, qz;
if (trace > 0) {
double S = sqrt(trace + 1.0) * 2; // S=4*qw
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11)&(m00 > m22)) {
double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
qx = 0.25 * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
} else if (m11 > m22) {
double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
qx = (m01 + m10) / S;
qy = 0.25 * S;
qz = (m12 + m21) / S;
} else {
double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25 * S;
}
return (Mat_<double>(3,1) << qx, qy, qz);
}
static Mat skew(const Mat& v)
{
CV_Assert(v.type() == CV_64FC1 && v.rows == 3 && v.cols == 1);
double vx = v.at<double>(0,0);
double vy = v.at<double>(1,0);
double vz = v.at<double>(2,0);
return (Mat_<double>(3,3) << 0, -vz, vy,
vz, 0, -vx,
-vy, vx, 0);
}
// R = quatMinimal2rot(q)
//
// q - 3x1 unit quaternion <qx, qy, qz>
// R - 3x3 rotation matrix
// q = sin(theta/2) * v
// theta - rotation angle
// v - unit rotation axis, |v| = 1
static Mat quatMinimal2rot(const Mat& q)
{
CV_Assert(q.type() == CV_64FC1 && q.rows == 3 && q.cols == 1);
Mat p = q.t()*q;
double w = sqrt(1 - p.at<double>(0,0));
Mat diag_p = Mat::eye(3,3,CV_64FC1)*p.at<double>(0,0);
return 2*q*q.t() + 2*w*skew(q) + Mat::eye(3,3,CV_64FC1) - 2*diag_p;
}
// q = rot2quat(R)
//
// q - 4x1 unit quaternion <qw, qx, qy, qz>
// R - 3x3 rotation matrix
static Mat rot2quat(const Mat& R)
{
CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
double trace = m00 + m11 + m22;
double qw, qx, qy, qz;
if (trace > 0) {
double S = sqrt(trace + 1.0) * 2; // S=4*qw
qw = 0.25 * S;
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11)&(m00 > m22)) {
double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
qw = (m21 - m12) / S;
qx = 0.25 * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
} else if (m11 > m22) {
double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
qw = (m02 - m20) / S;
qx = (m01 + m10) / S;
qy = 0.25 * S;
qz = (m12 + m21) / S;
} else {
double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
qw = (m10 - m01) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25 * S;
}
return (Mat_<double>(4,1) << qw, qx, qy, qz);
}
// R = quat2rot(q)
//
// q - 4x1 unit quaternion <qw, qx, qy, qz>
// R - 3x3 rotation matrix
static Mat quat2rot(const Mat& q)
{
CV_Assert(q.type() == CV_64FC1 && q.rows == 4 && q.cols == 1);
double qw = q.at<double>(0,0);
double qx = q.at<double>(1,0);
double qy = q.at<double>(2,0);
double qz = q.at<double>(3,0);
Mat R(3, 3, CV_64FC1);
R.at<double>(0, 0) = 1 - 2*qy*qy - 2*qz*qz;
R.at<double>(0, 1) = 2*qx*qy - 2*qz*qw;
R.at<double>(0, 2) = 2*qx*qz + 2*qy*qw;
R.at<double>(1, 0) = 2*qx*qy + 2*qz*qw;
R.at<double>(1, 1) = 1 - 2*qx*qx - 2*qz*qz;
R.at<double>(1, 2) = 2*qy*qz - 2*qx*qw;
R.at<double>(2, 0) = 2*qx*qz - 2*qy*qw;
R.at<double>(2, 1) = 2*qy*qz + 2*qx*qw;
R.at<double>(2, 2) = 1 - 2*qx*qx - 2*qy*qy;
return R;
}
// Kronecker product or tensor product
// https://stackoverflow.com/a/36552682
static Mat kron(const Mat& A, const Mat& B)
{
CV_Assert(A.channels() == 1 && B.channels() == 1);
Mat1d Ad, Bd;
A.convertTo(Ad, CV_64F);
B.convertTo(Bd, CV_64F);
Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0);
for (int ra = 0; ra < Ad.rows; ra++)
{
for (int ca = 0; ca < Ad.cols; ca++)
{
Kd(Range(ra*Bd.rows, (ra + 1)*Bd.rows), Range(ca*Bd.cols, (ca + 1)*Bd.cols)) = Bd.mul(Ad(ra, ca));
}
}
Mat K;
Kd.convertTo(K, A.type());
return K;
}
// quaternion multiplication
static Mat qmult(const Mat& s, const Mat& t)
{
CV_Assert(s.type() == CV_64FC1 && t.type() == CV_64FC1);
CV_Assert(s.rows == 4 && s.cols == 1);
CV_Assert(t.rows == 4 && t.cols == 1);
double s0 = s.at<double>(0,0);
double s1 = s.at<double>(1,0);
double s2 = s.at<double>(2,0);
double s3 = s.at<double>(3,0);
double t0 = t.at<double>(0,0);
double t1 = t.at<double>(1,0);
double t2 = t.at<double>(2,0);
double t3 = t.at<double>(3,0);
Mat q(4, 1, CV_64FC1);
q.at<double>(0,0) = s0*t0 - s1*t1 - s2*t2 - s3*t3;
q.at<double>(1,0) = s0*t1 + s1*t0 + s2*t3 - s3*t2;
q.at<double>(2,0) = s0*t2 - s1*t3 + s2*t0 + s3*t1;
q.at<double>(3,0) = s0*t3 + s1*t2 - s2*t1 + s3*t0;
return q;
}
// dq = homogeneous2dualQuaternion(H)
//
// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
static Mat homogeneous2dualQuaternion(const Mat& H)
{
CV_Assert(H.type() == CV_64FC1 && H.rows == 4 && H.cols == 4);
Mat dualq(8, 1, CV_64FC1);
Mat R = H(Rect(0, 0, 3, 3));
Mat t = H(Rect(3, 0, 1, 3));
Mat q = rot2quat(R);
Mat qt = Mat::zeros(4, 1, CV_64FC1);
t.copyTo(qt(Rect(0, 1, 1, 3)));
Mat qprime = 0.5 * qmult(qt, q);
q.copyTo(dualq(Rect(0, 0, 1, 4)));
qprime.copyTo(dualq(Rect(0, 4, 1, 4)));
return dualq;
}
// H = dualQuaternion2homogeneous(dq)
//
// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
// dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
static Mat dualQuaternion2homogeneous(const Mat& dualq)
{
CV_Assert(dualq.type() == CV_64FC1 && dualq.rows == 8 && dualq.cols == 1);
Mat q = dualq(Rect(0, 0, 1, 4));
Mat qprime = dualq(Rect(0, 4, 1, 4));
Mat R = quat2rot(q);
q.at<double>(1,0) = -q.at<double>(1,0);
q.at<double>(2,0) = -q.at<double>(2,0);
q.at<double>(3,0) = -q.at<double>(3,0);
Mat qt = 2*qmult(qprime, q);
Mat t = qt(Rect(0, 1, 1, 3));
Mat H = Mat::eye(4, 4, CV_64FC1);
R.copyTo(H(Rect(0, 0, 3, 3)));
t.copyTo(H(Rect(3, 0, 1, 3)));
return H;
}
//Reference:
//R. Y. Tsai and R. K. Lenz, "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration."
//In IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989.
//C++ code converted from Zoran Lazarevic's Matlab code:
//http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m
static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
//Number of unique camera position pairs
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
//Will store: skew(Pgij+Pcij)
Mat A(3*K, 3, CV_64FC1);
//Will store: Pcij - Pgij
Mat B(3*K, 1, CV_64FC1);
std::vector<Mat> vec_Hgij, vec_Hcij;
vec_Hgij.reserve(static_cast<size_t>(K));
vec_Hcij.reserve(static_cast<size_t>(K));
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6
vec_Hgij.push_back(Hgij);
//Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj
Mat Pgij = 2*rot2quatMinimal(Hgij);
//Defines coordinate transformation from Ci to Cj
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); //eq 7
vec_Hcij.push_back(Hcij);
//Rotation axis for Rcij
Mat Pcij = 2*rot2quatMinimal(Hcij);
//Left-hand side: skew(Pgij+Pcij)
skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3)));
//Right-hand side: Pcij - Pgij
Mat diff = Pcij - Pgij;
diff.copyTo(B(Rect(0, idx*3, 1, 3)));
}
}
Mat Pcg_;
//Rotation from camera to gripper is obtained from the set of equations:
// skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12)
solve(A, B, Pcg_, DECOMP_SVD);
Mat Pcg_norm = Pcg_.t() * Pcg_;
//Obtained non-unit quaternion is scaled back to unit value that
//designates camera-gripper rotation
Mat Pcg = 2 * Pcg_ / sqrt(1 + Pcg_norm.at<double>(0,0)); //eq 14
Mat Rcg = quatMinimal2rot(Pcg/2.0);
idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
//Defines coordinate transformation from Ci to Cj
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
//Left-hand side: (Rgij - I)
Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
diff.copyTo(A(Rect(0, idx*3, 3, 3)));
//Right-hand side: Rcg*Tcij - Tgij
diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
diff.copyTo(B(Rect(0, idx*3, 1, 3)));
}
}
Mat Tcg;
//Translation from camera to gripper is obtained from the set of equations:
// (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15)
solve(A, B, Tcg, DECOMP_SVD);
R_cam2gripper = Rcg;
t_cam2gripper = Tcg;
}
//Reference:
//F. Park, B. Martin, "Robot Sensor Calibration: Solving AX = XB on the Euclidean Group."
//In IEEE Transactions on Robotics and Automation, 10(5): 717-721, 1994.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyePark(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
Mat M = Mat::zeros(3, 3, CV_64FC1);
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat a, b;
Rodrigues(Rgij, a);
Rodrigues(Rcij, b);
M += b * a.t();
}
}
Mat eigenvalues, eigenvectors;
eigen(M.t()*M, eigenvalues, eigenvectors);
Mat v = Mat::zeros(3, 3, CV_64FC1);
for (int i = 0; i < 3; i++) {
v.at<double>(i,i) = 1.0 / sqrt(eigenvalues.at<double>(i,0));
}
Mat R = eigenvectors.t() * v * eigenvectors * M.t();
R_cam2gripper = R;
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat C(3*K, 3, CV_64FC1);
Mat d(3*K, 1, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
Mat I_tgij = I3 - Rgij;
I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
Mat A_RB = tgij - R*tcij;
A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
}
}
Mat t;
solve(C, d, t, DECOMP_SVD);
t_cam2gripper = t;
}
//Reference:
//R. Horaud, F. Dornaika, "Hand-Eye Calibration"
//In International Journal of Robotics Research, 14(3): 195-210, 1995.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
Mat A = Mat::zeros(4, 4, CV_64FC1);
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat qgij = rot2quat(Rgij);
double r0 = qgij.at<double>(0,0);
double rx = qgij.at<double>(1,0);
double ry = qgij.at<double>(2,0);
double rz = qgij.at<double>(3,0);
// Q(r) Appendix A
Matx44d Qvi(r0, -rx, -ry, -rz,
rx, r0, -rz, ry,
ry, rz, r0, -rx,
rz, -ry, rx, r0);
Mat qcij = rot2quat(Rcij);
r0 = qcij.at<double>(0,0);
rx = qcij.at<double>(1,0);
ry = qcij.at<double>(2,0);
rz = qcij.at<double>(3,0);
// W(r) Appendix A
Matx44d Wvi(r0, -rx, -ry, -rz,
rx, r0, rz, -ry,
ry, -rz, r0, rx,
rz, ry, -rx, r0);
// Ai = (Q(vi') - W(vi))^T (Q(vi') - W(vi))
A += (Qvi - Wvi).t() * (Qvi - Wvi);
}
}
Mat eigenvalues, eigenvectors;
eigen(A, eigenvalues, eigenvectors);
Mat R = quat2rot(eigenvectors.row(3).t());
R_cam2gripper = R;
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat C(3*K, 3, CV_64FC1);
Mat d(3*K, 1, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
Mat I_tgij = I3 - Rgij;
I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
Mat A_RB = tgij - R*tcij;
A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
}
}
Mat t;
solve(C, d, t, DECOMP_SVD);
t_cam2gripper = t;
}
// sign function, return -1 if negative values, +1 otherwise
static int sign_double(double val)
{
return (0 < val) - (val < 0);
}
//Reference:
//N. Andreff, R. Horaud, B. Espiau, "On-line Hand-Eye Calibration."
//In Second International Conference on 3-D Digital Imaging and Modeling (3DIM'99), pages 430-436, 1999.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeAndreff(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat A(12*K, 12, CV_64FC1);
Mat B(12*K, 1, CV_64FC1);
Mat I9 = Mat::eye(9, 9, CV_64FC1);
Mat I3 = Mat::eye(3, 3, CV_64FC1);
Mat O9x3 = Mat::zeros(9, 3, CV_64FC1);
Mat O9x1 = Mat::zeros(9, 1, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat Rgij = Hgij(Rect(0, 0, 3, 3));
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat tgij = Hgij(Rect(3, 0, 1, 3));
Mat tcij = Hcij(Rect(3, 0, 1, 3));
//Eq 10
Mat a00 = I9 - kron(Rgij, Rcij);
Mat a01 = O9x3;
Mat a10 = kron(I3, tcij.t());
Mat a11 = I3 - Rgij;
a00.copyTo(A(Rect(0, idx*12, 9, 9)));
a01.copyTo(A(Rect(9, idx*12, 3, 9)));
a10.copyTo(A(Rect(0, idx*12 + 9, 9, 3)));
a11.copyTo(A(Rect(9, idx*12 + 9, 3, 3)));
O9x1.copyTo(B(Rect(0, idx*12, 1, 9)));
tgij.copyTo(B(Rect(0, idx*12 + 9, 1, 3)));
}
}
Mat X;
solve(A, B, X, DECOMP_SVD);
Mat R = X(Rect(0, 0, 1, 9));
int newSize[] = {3, 3};
R = R.reshape(1, 2, newSize);
//Eq 15
double det = determinant(R);
R = pow(sign_double(det) / abs(det), 1.0/3.0) * R;
Mat w, u, vt;
SVDecomp(R, w, u, vt);
R = u*vt;
if (determinant(R) < 0)
{
Mat diag = (Mat_<double>(3,3) << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
R = u*diag*vt;
}
R_cam2gripper = R;
Mat t = X(Rect(0, 9, 1, 3));
t_cam2gripper = t;
}
//Reference:
//K. Daniilidis, "Hand-Eye Calibration Using Dual Quaternions."
//In The International Journal of Robotics Research,18(3): 286-298, 1998.
//Matlab code: http://math.loyola.edu/~mili/Calibration/
static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
Mat& R_cam2gripper, Mat& t_cam2gripper)
{
int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
Mat T = Mat::zeros(6*K, 8, CV_64FC1);
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat dualqa = homogeneous2dualQuaternion(Hgij);
Mat dualqb = homogeneous2dualQuaternion(Hcij);
Mat a = dualqa(Rect(0, 1, 1, 3));
Mat b = dualqb(Rect(0, 1, 1, 3));
Mat aprime = dualqa(Rect(0, 5, 1, 3));
Mat bprime = dualqb(Rect(0, 5, 1, 3));
//Eq 31
Mat s00 = a - b;
Mat s01 = skew(a + b);
Mat s10 = aprime - bprime;
Mat s11 = skew(aprime + bprime);
Mat s12 = a - b;
Mat s13 = skew(a + b);
s00.copyTo(T(Rect(0, idx*6, 1, 3)));
s01.copyTo(T(Rect(1, idx*6, 3, 3)));
s10.copyTo(T(Rect(0, idx*6 + 3, 1, 3)));
s11.copyTo(T(Rect(1, idx*6 + 3, 3, 3)));
s12.copyTo(T(Rect(4, idx*6 + 3, 1, 3)));
s13.copyTo(T(Rect(5, idx*6 + 3, 3, 3)));
}
}
Mat w, u, vt;
SVDecomp(T, w, u, vt);
Mat v = vt.t();
Mat u1 = v(Rect(6, 0, 1, 4));
Mat v1 = v(Rect(6, 4, 1, 4));
Mat u2 = v(Rect(7, 0, 1, 4));
Mat v2 = v(Rect(7, 4, 1, 4));
//Solves Eq 34, Eq 35
Mat ma = u1.t()*v1;
Mat mb = u1.t()*v2 + u2.t()*v1;
Mat mc = u2.t()*v2;
double a = ma.at<double>(0,0);
double b = mb.at<double>(0,0);
double c = mc.at<double>(0,0);
double s1 = (-b + sqrt(b*b - 4*a*c)) / (2*a);
double s2 = (-b - sqrt(b*b - 4*a*c)) / (2*a);
Mat sol1 = s1*s1*u1.t()*u1 + 2*s1*u1.t()*u2 + u2.t()*u2;
Mat sol2 = s2*s2*u1.t()*u1 + 2*s2*u1.t()*u2 + u2.t()*u2;
double s, val;
if (sol1.at<double>(0,0) > sol2.at<double>(0,0))
{
s = s1;
val = sol1.at<double>(0,0);
}
else
{
s = s2;
val = sol2.at<double>(0,0);
}
double lambda2 = sqrt(1.0 / val);
double lambda1 = s * lambda2;
Mat dualq = lambda1 * v(Rect(6, 0, 1, 8)) + lambda2*v(Rect(7, 0, 1, 8));
Mat X = dualQuaternion2homogeneous(dualq);
Mat R = X(Rect(0, 0, 3, 3));
Mat t = X(Rect(3, 0, 1, 3));
R_cam2gripper = R;
t_cam2gripper = t;
}
void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
OutputArray R_cam2gripper, OutputArray t_cam2gripper,
HandEyeCalibrationMethod method)
{
CV_Assert(R_gripper2base.isMatVector() && t_gripper2base.isMatVector() &&
R_target2cam.isMatVector() && t_target2cam.isMatVector());
std::vector<Mat> R_gripper2base_, t_gripper2base_;
R_gripper2base.getMatVector(R_gripper2base_);
t_gripper2base.getMatVector(t_gripper2base_);
std::vector<Mat> R_target2cam_, t_target2cam_;
R_target2cam.getMatVector(R_target2cam_);
t_target2cam.getMatVector(t_target2cam_);
CV_Assert(R_gripper2base_.size() == t_gripper2base_.size() &&
R_target2cam_.size() == t_target2cam_.size() &&
R_gripper2base_.size() == R_target2cam_.size());
CV_Assert(R_gripper2base_.size() >= 3);
//Notation used in Tsai paper
//Defines coordinate transformation from G (gripper) to RW (robot base)
std::vector<Mat> Hg;
Hg.reserve(R_gripper2base_.size());
for (size_t i = 0; i < R_gripper2base_.size(); i++)
{
Mat m = Mat::eye(4, 4, CV_64FC1);
Mat R = m(Rect(0, 0, 3, 3));
R_gripper2base_[i].convertTo(R, CV_64F);
Mat t = m(Rect(3, 0, 1, 3));
t_gripper2base_[i].convertTo(t, CV_64F);
Hg.push_back(m);
}
//Defines coordinate transformation from CW (calibration target) to C (camera)
std::vector<Mat> Hc;
Hc.reserve(R_target2cam_.size());
for (size_t i = 0; i < R_target2cam_.size(); i++)
{
Mat m = Mat::eye(4, 4, CV_64FC1);
Mat R = m(Rect(0, 0, 3, 3));
R_target2cam_[i].convertTo(R, CV_64F);
Mat t = m(Rect(3, 0, 1, 3));
t_target2cam_[i].convertTo(t, CV_64F);
Hc.push_back(m);
}
Mat Rcg = Mat::eye(3, 3, CV_64FC1);
Mat Tcg = Mat::zeros(3, 1, CV_64FC1);
switch (method)
{
case CALIB_HAND_EYE_TSAI:
calibrateHandEyeTsai(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_PARK:
calibrateHandEyePark(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_HORAUD:
calibrateHandEyeHoraud(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_ANDREFF:
calibrateHandEyeAndreff(Hg, Hc, Rcg, Tcg);
break;
case CALIB_HAND_EYE_DANIILIDIS:
calibrateHandEyeDaniilidis(Hg, Hc, Rcg, Tcg);
break;
default:
break;
}
Rcg.copyTo(R_cam2gripper);
Tcg.copyTo(t_cam2gripper);
}
}

226
Lib/opencv/sources/modules/calib3d/src/checkchessboard.cpp Normal file
View File

@@ -0,0 +1,226 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "calib3d_c_api.h"
#include <vector>
#include <algorithm>
using namespace cv;
using namespace std;
static void icvGetQuadrangleHypotheses(const std::vector<std::vector< cv::Point > > & contours, const std::vector< cv::Vec4i > & hierarchy, std::vector<std::pair<float, int> >& quads, int class_id)
{
const float min_aspect_ratio = 0.3f;
const float max_aspect_ratio = 3.0f;
const float min_box_size = 10.0f;
typedef std::vector< std::vector< cv::Point > >::const_iterator iter_t;
iter_t i;
for (i = contours.begin(); i != contours.end(); ++i)
{
const iter_t::difference_type idx = i - contours.begin();
if (hierarchy.at(idx)[3] != -1)
continue; // skip holes
const std::vector< cv::Point > & c = *i;
cv::RotatedRect box = cv::minAreaRect(c);
float box_size = MAX(box.size.width, box.size.height);
if(box_size < min_box_size)
{
continue;
}
float aspect_ratio = box.size.width/MAX(box.size.height, 1);
if(aspect_ratio < min_aspect_ratio || aspect_ratio > max_aspect_ratio)
{
continue;
}
quads.push_back(std::pair<float, int>(box_size, class_id));
}
}
static void countClasses(const std::vector<std::pair<float, int> >& pairs, size_t idx1, size_t idx2, std::vector<int>& counts)
{
counts.assign(2, 0);
for(size_t i = idx1; i != idx2; i++)
{
counts[pairs[i].second]++;
}
}
inline bool less_pred(const std::pair<float, int>& p1, const std::pair<float, int>& p2)
{
return p1.first < p2.first;
}
static void fillQuads(Mat & white, Mat & black, double white_thresh, double black_thresh, vector<pair<float, int> > & quads)
{
Mat thresh;
{
vector< vector<Point> > contours;
vector< Vec4i > hierarchy;
threshold(white, thresh, white_thresh, 255, THRESH_BINARY);
findContours(thresh, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
icvGetQuadrangleHypotheses(contours, hierarchy, quads, 1);
}
{
vector< vector<Point> > contours;
vector< Vec4i > hierarchy;
threshold(black, thresh, black_thresh, 255, THRESH_BINARY_INV);
findContours(thresh, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
icvGetQuadrangleHypotheses(contours, hierarchy, quads, 0);
}
}
static bool checkQuads(vector<pair<float, int> > & quads, const cv::Size & size)
{
const size_t min_quads_count = size.width*size.height/2;
std::sort(quads.begin(), quads.end(), less_pred);
// now check if there are many hypotheses with similar sizes
// do this by floodfill-style algorithm
const float size_rel_dev = 0.4f;
for(size_t i = 0; i < quads.size(); i++)
{
size_t j = i + 1;
for(; j < quads.size(); j++)
{
if(quads[j].first/quads[i].first > 1.0f + size_rel_dev)
{
break;
}
}
if(j + 1 > min_quads_count + i)
{
// check the number of black and white squares
std::vector<int> counts;
countClasses(quads, i, j, counts);
const int black_count = cvRound(ceil(size.width/2.0)*ceil(size.height/2.0));
const int white_count = cvRound(floor(size.width/2.0)*floor(size.height/2.0));
if(counts[0] < black_count*0.75 ||
counts[1] < white_count*0.75)
{
continue;
}
return true;
}
}
return false;
}
// does a fast check if a chessboard is in the input image. This is a workaround to
// a problem of cvFindChessboardCorners being slow on images with no chessboard
// - src: input image
// - size: chessboard size
// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
// 0 if there is no chessboard, -1 in case of error
int cvCheckChessboard(IplImage* src, CvSize size)
{
cv::Mat img = cv::cvarrToMat(src);
return (int)cv::checkChessboard(img, size);
}
bool cv::checkChessboard(InputArray _img, Size size)
{
Mat img = _img.getMat();
CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
const int erosion_count = 1;
const float black_level = 20.f;
const float white_level = 130.f;
const float black_white_gap = 70.f;
Mat white;
Mat black;
erode(img, white, Mat(), Point(-1, -1), erosion_count);
dilate(img, black, Mat(), Point(-1, -1), erosion_count);
bool result = false;
for(float thresh_level = black_level; thresh_level < white_level && !result; thresh_level += 20.0f)
{
vector<pair<float, int> > quads;
fillQuads(white, black, thresh_level + black_white_gap, thresh_level, quads);
if (checkQuads(quads, size))
result = true;
}
return result;
}
// does a fast check if a chessboard is in the input image. This is a workaround to
// a problem of cvFindChessboardCorners being slow on images with no chessboard
// - src: input binary image
// - size: chessboard size
// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
// 0 if there is no chessboard, -1 in case of error
int checkChessboardBinary(const cv::Mat & img, const cv::Size & size)
{
CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
Mat white = img.clone();
Mat black = img.clone();
int result = 0;
for ( int erosion_count = 0; erosion_count <= 3; erosion_count++ )
{
if ( 1 == result )
break;
if ( 0 != erosion_count ) // first iteration keeps original images
{
erode(white, white, Mat(), Point(-1, -1), 1);
dilate(black, black, Mat(), Point(-1, -1), 1);
}
vector<pair<float, int> > quads;
fillQuads(white, black, 128, 128, quads);
if (checkQuads(quads, size))
result = 1;
}
return result;
}

3219
Lib/opencv/sources/modules/calib3d/src/chessboard.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

768
Lib/opencv/sources/modules/calib3d/src/chessboard.hpp Normal file
View File

@@ -0,0 +1,768 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef CHESSBOARD_HPP_
#define CHESSBOARD_HPP_
#include "opencv2/core.hpp"
#include "opencv2/features2d.hpp"
#include <vector>
#include <set>
#include <map>
namespace cv {
namespace details{
/**
* \brief Fast point sysmetric cross detector based on a localized radon transformation
*/
class FastX : public cv::Feature2D
{
public:
struct Parameters
{
float strength; //!< minimal strength of a valid junction in dB
float resolution; //!< angle resolution in radians
int branches; //!< the number of branches
int min_scale; //!< scale level [0..8]
int max_scale; //!< scale level [0..8]
bool filter; //!< post filter feature map to improve impulse response
bool super_resolution; //!< up-sample
Parameters()
{
strength = 40;
resolution = float(CV_PI*0.25);
branches = 2;
min_scale = 2;
max_scale = 5;
super_resolution = 1;
filter = true;
}
};
public:
FastX(const Parameters &config = Parameters());
virtual ~FastX(){};
void reconfigure(const Parameters &para);
//declaration to be wrapped by rbind
void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
{cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
virtual void detectAndCompute(cv::InputArray image,
cv::InputArray mask,
std::vector<cv::KeyPoint>& keypoints,
cv::OutputArray descriptors,
bool useProvidedKeyPoints = false)override;
void detectImpl(const cv::Mat& image,
std::vector<cv::KeyPoint>& keypoints,
std::vector<cv::Mat> &feature_maps,
const cv::Mat& mask=cv::Mat())const;
void detectImpl(const cv::Mat& image,
std::vector<cv::Mat> &rotated_images,
std::vector<cv::Mat> &feature_maps,
const cv::Mat& mask=cv::Mat())const;
void findKeyPoints(const std::vector<cv::Mat> &feature_map,
std::vector<cv::KeyPoint>& keypoints,
const cv::Mat& mask = cv::Mat())const;
std::vector<std::vector<float> > calcAngles(const std::vector<cv::Mat> &rotated_images,
std::vector<cv::KeyPoint> &keypoints)const;
// define pure virtual methods
virtual int descriptorSize()const override{return 0;};
virtual int descriptorType()const override{return 0;};
virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
{
descriptors.clear();
detectImpl(image.getMat(),keypoints,mask);
if(!useProvidedKeypoints) // suppress compiler warning
return;
return;
}
protected:
virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
{
descriptors = cv::Mat();
detectImpl(image,keypoints);
}
private:
void detectImpl(const cv::Mat& _src, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask)const;
virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
void rotate(float angle,const cv::Mat &img,cv::Size size,cv::Mat &out)const;
void calcFeatureMap(const cv::Mat &images,cv::Mat& out)const;
private:
Parameters parameters;
};
/**
* \brief Ellipse class
*/
class Ellipse
{
public:
Ellipse();
Ellipse(const cv::Point2f &center, const cv::Size2f &axes, float angle);
void draw(cv::InputOutputArray img,const cv::Scalar &color = cv::Scalar::all(120))const;
bool contains(const cv::Point2f &pt)const;
cv::Point2f getCenter()const;
const cv::Size2f &getAxes()const;
private:
cv::Point2f center;
cv::Size2f axes;
float angle,cosf,sinf;
};
/**
* \brief Chessboard corner detector
*
* The detectors tries to find all chessboard corners of an imaged
* chessboard and returns them as an ordered vector of KeyPoints.
* Thereby, the left top corner has index 0 and the bottom right
* corner n*m-1.
*/
class Chessboard: public cv::Feature2D
{
public:
static const int DUMMY_FIELD_SIZE = 100; // in pixel
/**
* \brief Configuration of a chessboard corner detector
*
*/
struct Parameters
{
cv::Size chessboard_size; //!< size of the chessboard
int min_scale; //!< scale level [0..8]
int max_scale; //!< scale level [0..8]
int max_points; //!< maximal number of points regarded
int max_tests; //!< maximal number of tested hypothesis
bool super_resolution; //!< use super-repsolution for chessboard detection
bool larger; //!< indicates if larger boards should be returned
Parameters()
{
chessboard_size = cv::Size(9,6);
min_scale = 2;
max_scale = 4;
super_resolution = true;
max_points = 400;
max_tests = 100;
larger = false;
}
Parameters(int scale,int _max_points):
min_scale(scale),
max_scale(scale),
max_points(_max_points)
{
chessboard_size = cv::Size(9,6);
}
};
/**
* \brief Gets the 3D objects points for the chessboard assuming the
* left top corner is located at the origin.
*
* \param[in] pattern_size Number of rows and cols of the pattern
* \param[in] cell_size Size of one cell
*
* \returns Returns the object points as CV_32FC3
*/
static cv::Mat getObjectPoints(const cv::Size &pattern_size,float cell_size);
/**
* \brief Class for searching and storing chessboard corners.
*
* The search is based on a feature map having strong pixel
* values at positions where a chessboard corner is located.
*
* The board must be rectangular but supports empty cells
*
*/
class Board
{
public:
/**
* \brief Estimates the position of the next point on a line using cross ratio constrain
*
* cross ratio:
* d12/d34 = d13/d24
*
* point order on the line:
* pt1 --> pt2 --> pt3 --> pt4
*
* \param[in] pt1 First point coordinate
* \param[in] pt2 Second point coordinate
* \param[in] pt3 Third point coordinate
* \param[out] pt4 Forth point coordinate
*
*/
static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,cv::Point2f &p3);
// using 1D homography
static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3, cv::Point2f &p4);
/**
* \brief Checks if all points of a row or column have a valid cross ratio constraint
*
* cross ratio:
* d12/d34 = d13/d24
*
* point order on the row/column:
* pt1 --> pt2 --> pt3 --> pt4
*
* \param[in] points THe points of the row/column
*
*/
static bool checkRowColumn(const std::vector<cv::Point2f> &points);
/**
* \brief Estimates the search area for the next point on the line using cross ratio
*
* point order on the line:
* (p0) --> p1 --> p2 --> p3 --> search area
*
* \param[in] p1 First point coordinate
* \param[in] p2 Second point coordinate
* \param[in] p3 Third point coordinate
* \param[in] p Percentage of d34 used for the search area width and height [0..1]
* \param[out] ellipse The search area
* \param[in] p0 optional point to improve accuracy
*
* \return Returns false if no search area can be calculated
*
*/
static bool estimateSearchArea(const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3,float p,
Ellipse &ellipse,const cv::Point2f *p0 =NULL);
/**
* \brief Estimates the search area for a specific point based on the given homography
*
* \param[in] H homography describing the transformation from ideal board to real one
* \param[in] row Row of the point
* \param[in] col Col of the point
* \param[in] p Percentage [0..1]
*
* \return Returns false if no search area can be calculated
*
*/
static Ellipse estimateSearchArea(cv::Mat H,int row, int col,float p,int field_size = DUMMY_FIELD_SIZE);
/**
* \brief Searches for the maximum in a given search area
*
* \param[in] map feature map
* \param[in] ellipse search area
* \param[in] min_val Minimum value of the maximum to be accepted as maximum
*
* \return Returns a negative value if all points are outside the ellipse
*
*/
static float findMaxPoint(cv::flann::Index &index,const cv::Mat &data,const Ellipse &ellipse,float white_angle,float black_angle,cv::Point2f &pt);
/**
* \brief Searches for the next point using cross ratio constrain
*
* \param[in] index flann index
* \param[in] data extended flann data
* \param[in] pt1
* \param[in] pt2
* \param[in] pt3
* \param[in] white_angle
* \param[in] black_angle
* \param[in] min_response
* \param[out] point The resulting point
*
* \return Returns false if no point could be found
*
*/
static bool findNextPoint(cv::flann::Index &index,const cv::Mat &data,
const cv::Point2f &pt1,const cv::Point2f &pt2, const cv::Point2f &pt3,
float white_angle,float black_angle,float min_response,cv::Point2f &point);
/**
* \brief Creates a new Board object
*
*/
Board(float white_angle=0,float black_angle=0);
Board(const cv::Size &size, const std::vector<cv::Point2f> &points,float white_angle=0,float black_angle=0);
Board(const Chessboard::Board &other);
virtual ~Board();
Board& operator=(const Chessboard::Board &other);
/**
* \brief Draws the corners into the given image
*
* \param[in] m The image
* \param[out] m The resulting image
* \param[in] H optional homography to calculate search area
*
*/
void draw(cv::InputArray m,cv::OutputArray out,cv::InputArray H=cv::Mat())const;
/**
* \brief Estimates the pose of the chessboard
*
*/
bool estimatePose(const cv::Size2f &real_size,cv::InputArray _K,cv::OutputArray rvec,cv::OutputArray tvec)const;
/**
* \brief Clears all internal data of the object
*
*/
void clear();
/**
* \brief Returns the angle of the black diagnonale
*
*/
float getBlackAngle()const;
/**
* \brief Returns the angle of the black diagnonale
*
*/
float getWhiteAngle()const;
/**
* \brief Initializes a 3x3 grid from 9 corner coordinates
*
* All points must be ordered:
* p0 p1 p2
* p3 p4 p5
* p6 p7 p8
*
* \param[in] points vector of points
*
* \return Returns false if the grid could not be initialized
*/
bool init(const std::vector<cv::Point2f> points);
/**
* \brief Returns true if the board is empty
*
*/
bool isEmpty() const;
/**
* \brief Returns all board corners as ordered vector
*
* The left top corner has index 0 and the bottom right
* corner rows*cols-1. All corners which only belong to
* empty cells are returned as NaN.
*/
std::vector<cv::Point2f> getCorners(bool ball=true) const;
/**
* \brief Returns all board corners as ordered vector of KeyPoints
*
* The left top corner has index 0 and the bottom right
* corner rows*cols-1.
*
* \param[in] ball if set to false only non empty points are returned
*
*/
std::vector<cv::KeyPoint> getKeyPoints(bool ball=true) const;
/**
* \brief Returns the centers of the chessboard cells
*
* The left top corner has index 0 and the bottom right
* corner (rows-1)*(cols-1)-1.
*
*/
std::vector<cv::Point2f> getCellCenters() const;
/**
* \brief Estimates the homography between an ideal board
* and reality based on the already recovered points
*
* \param[in] rect selecting a subset of the already recovered points
* \param[in] field_size The field size of the ideal board
*
*/
cv::Mat estimateHomography(cv::Rect rect,int field_size = DUMMY_FIELD_SIZE)const;
/**
* \brief Estimates the homography between an ideal board
* and reality based on the already recovered points
*
* \param[in] field_size The field size of the ideal board
*
*/
cv::Mat estimateHomography(int field_size = DUMMY_FIELD_SIZE)const;
/**
* \brief Returns the size of the board
*
*/
cv::Size getSize() const;
/**
* \brief Returns the number of cols
*
*/
size_t colCount() const;
/**
* \brief Returns the number of rows
*
*/
size_t rowCount() const;
/**
* \brief Returns the inner contour of the board including only valid corners
*
* \info the contour might be non squared if not all points of the board are defined
*
*/
std::vector<cv::Point2f> getContour()const;
/**
* \brief Grows the board in all direction until no more corners are found in the feature map
*
* \param[in] data CV_32FC1 data of the flann index
* \param[in] flann_index flann index
*
* \returns the number of grows
*/
int grow(const cv::Mat &data,cv::flann::Index &flann_index);
/**
* \brief Validates all corners using guided search based on the given homography
*
* \param[in] data CV_32FC1 data of the flann index
* \param[in] flann_index flann index
* \param[in] h Homography describing the transformation from ideal board to the real one
* \param[in] min_response Min response
*
* \returns the number of valid corners
*/
int validateCorners(const cv::Mat &data,cv::flann::Index &flann_index,const cv::Mat &h,float min_response=0);
/**
* \brief check that no corner is used more than once
*
* \returns Returns false if a corner is used more than once
*/
bool checkUnique()const;
/**
* \brief Returns false if the angles of the contour are smaller than 35°
*
*/
bool validateContour()const;
/**
* \brief Grows the board to the left by adding one column.
*
* \param[in] map CV_32FC1 feature map
*
* \returns Returns false if the feature map has no maxima at the requested positions
*/
bool growLeft(const cv::Mat &map,cv::flann::Index &flann_index);
void growLeft();
/**
* \brief Grows the board to the top by adding one row.
*
* \param[in] map CV_32FC1 feature map
*
* \returns Returns false if the feature map has no maxima at the requested positions
*/
bool growTop(const cv::Mat &map,cv::flann::Index &flann_index);
void growTop();
/**
* \brief Grows the board to the right by adding one column.
*
* \param[in] map CV_32FC1 feature map
*
* \returns Returns false if the feature map has no maxima at the requested positions
*/
bool growRight(const cv::Mat &map,cv::flann::Index &flann_index);
void growRight();
/**
* \brief Grows the board to the bottom by adding one row.
*
* \param[in] map CV_32FC1 feature map
*
* \returns Returns false if the feature map has no maxima at the requested positions
*/
bool growBottom(const cv::Mat &map,cv::flann::Index &flann_index);
void growBottom();
/**
* \brief Adds one column on the left side
*
* \param[in] points The corner coordinates
*
*/
void addColumnLeft(const std::vector<cv::Point2f> &points);
/**
* \brief Adds one column at the top
*
* \param[in] points The corner coordinates
*
*/
void addRowTop(const std::vector<cv::Point2f> &points);
/**
* \brief Adds one column on the right side
*
* \param[in] points The corner coordinates
*
*/
void addColumnRight(const std::vector<cv::Point2f> &points);
/**
* \brief Adds one row at the bottom
*
* \param[in] points The corner coordinates
*
*/
void addRowBottom(const std::vector<cv::Point2f> &points);
/**
* \brief Rotates the board 90° degrees to the left
*/
void rotateLeft();
/**
* \brief Rotates the board 90° degrees to the right
*/
void rotateRight();
/**
* \brief Flips the board along its local x(width) coordinate direction
*/
void flipVertical();
/**
* \brief Flips the board along its local y(height) coordinate direction
*/
void flipHorizontal();
/**
* \brief Flips and rotates the board so that the angle of
* either the black or white diagonal is bigger than the x
* and y axis of the board and from a right handed
* coordinate system
*/
void normalizeOrientation(bool bblack=true);
/**
* \brief Exchanges the stored board with the board stored in other
*/
void swap(Chessboard::Board &other);
bool operator==(const Chessboard::Board& other) const {return rows*cols == other.rows*other.cols;};
bool operator< (const Chessboard::Board& other) const {return rows*cols < other.rows*other.cols;};
bool operator> (const Chessboard::Board& other) const {return rows*cols > other.rows*other.cols;};
bool operator>= (const cv::Size& size)const { return rows*cols >= size.width*size.height; };
/**
* \brief Returns a specific corner
*
* \info raises runtime_error if row col does not exists
*/
cv::Point2f& getCorner(int row,int col);
/**
* \brief Returns true if the cell is empty meaning at least one corner is NaN
*/
bool isCellEmpty(int row,int col);
/**
* \brief Returns the mapping from all corners idx to only valid corners idx
*/
std::map<int,int> getMapping()const;
/**
* \brief Estimates rotation of the board around the camera axis
*/
double estimateRotZ()const;
/**
* \brief Returns true if the cell is black
*
*/
bool isCellBlack(int row,int cola)const;
private:
// stores one cell
// in general a cell is initialized by the Board so that:
// * all corners are always pointing to a valid cv::Point2f
// * depending on the position left,top,right and bottom might be set to NaN
// * A cell is empty if at least one corner is NaN
struct Cell
{
cv::Point2f *top_left,*top_right,*bottom_right,*bottom_left; // corners
Cell *left,*top,*right,*bottom; // neighbouring cells
bool black; // set to true if cell is black
Cell();
bool empty()const; // indicates if the cell is empty (one of its corners has NaN)
int getRow()const;
int getCol()const;
};
// corners
enum CornerIndex
{
TOP_LEFT,
TOP_RIGHT,
BOTTOM_RIGHT,
BOTTOM_LEFT
};
Cell* getCell(int row,int column); // returns a specific cell
const Cell* getCell(int row,int column)const; // returns a specific cell
void drawEllipses(const std::vector<Ellipse> &ellipses);
// Iterator for iterating over board corners
class PointIter
{
public:
PointIter(Cell *cell,CornerIndex corner_index);
PointIter(const PointIter &other);
void operator=(const PointIter &other);
bool valid() const; // returns if the pointer is pointing to a cell
bool left(bool check_empty=false); // moves one corner to the left or returns false
bool right(bool check_empty=false); // moves one corner to the right or returns false
bool bottom(bool check_empty=false); // moves one corner to the bottom or returns false
bool top(bool check_empty=false); // moves one corner to the top or returns false
bool checkCorner()const; // returns true if the current corner belongs to at least one
// none empty cell
bool isNaN()const; // returns true if the currnet corner is NaN
const cv::Point2f* operator*() const; // current corner coordinate
cv::Point2f* operator*(); // current corner coordinate
const cv::Point2f* operator->() const; // current corner coordinate
cv::Point2f* operator->(); // current corner coordinate
Cell *getCell(); // current cell
private:
CornerIndex corner_index;
Cell *cell;
};
std::vector<Cell*> cells; // storage for all board cells
std::vector<cv::Point2f*> corners; // storage for all corners
Cell *top_left; // pointer to the top left corner of the board in its local coordinate system
int rows; // number of row cells
int cols; // number of col cells
float white_angle,black_angle;
};
public:
/**
* \brief Creates a chessboard corner detectors
*
* \param[in] config Configuration used to detect chessboard corners
*
*/
Chessboard(const Parameters &config = Parameters());
virtual ~Chessboard();
void reconfigure(const Parameters &config = Parameters());
Parameters getPara()const;
/*
* \brief Detects chessboard corners in the given image.
*
* The detectors tries to find all chessboard corners of an imaged
* chessboard and returns them as an ordered vector of KeyPoints.
* Thereby, the left top corner has index 0 and the bottom right
* corner n*m-1.
*
* \param[in] image The image
* \param[out] keypoints The detected corners as a vector of ordered KeyPoints
* \param[in] mask Currently not supported
*
*/
void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
{cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
virtual void detectAndCompute(cv::InputArray image,cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors,
bool useProvidedKeyPoints = false)override;
/*
* \brief Detects chessboard corners in the given image.
*
* The detectors tries to find all chessboard corners of an imaged
* chessboard and returns them as an ordered vector of KeyPoints.
* Thereby, the left top corner has index 0 and the bottom right
* corner n*m-1.
*
* \param[in] image The image
* \param[out] keypoints The detected corners as a vector of ordered KeyPoints
* \param[out] feature_maps The feature map generated by LRJT and used to find the corners
* \param[in] mask Currently not supported
*
*/
void detectImpl(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
Chessboard::Board detectImpl(const cv::Mat& image,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
// define pure virtual methods
virtual int descriptorSize()const override{return 0;};
virtual int descriptorType()const override{return 0;};
virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
{
descriptors.clear();
detectImpl(image.getMat(),keypoints,mask);
if(!useProvidedKeypoints) // suppress compiler warning
return;
return;
}
protected:
virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
{
descriptors = cv::Mat();
detectImpl(image,keypoints);
}
// indicates why a board could not be initialized for a certain keypoint
enum BState
{
MISSING_POINTS = 0, // at least 5 points are needed
MISSING_PAIRS = 1, // at least two pairs are needed
WRONG_PAIR_ANGLE = 2, // angle between pairs is too small
WRONG_CONFIGURATION = 3, // point configuration is wrong and does not belong to a board
FOUND_BOARD = 4 // board was found
};
void findKeyPoints(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,
std::vector<std::vector<float> > &angles ,const cv::Mat& mask)const;
cv::Mat buildData(const std::vector<cv::KeyPoint>& keypoints)const;
std::vector<cv::KeyPoint> getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint &center,float white_angle,float black_angle, float min_response = 0)const;
BState generateBoards(cv::flann::Index &flann_index,const cv::Mat &data, const cv::KeyPoint &center,
float white_angle,float black_angle,float min_response,const cv::Mat &img,
std::vector<Chessboard::Board> &boards)const;
private:
void detectImpl(const cv::Mat&,std::vector<cv::KeyPoint>&, const cv::Mat& mast =cv::Mat())const;
virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
private:
Parameters parameters; // storing the configuration of the detector
};
}} // end namespace details and cv
#endif

1634
Lib/opencv/sources/modules/calib3d/src/circlesgrid.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

194
Lib/opencv/sources/modules/calib3d/src/circlesgrid.hpp Normal file
View File

@@ -0,0 +1,194 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef CIRCLESGRID_HPP_
#define CIRCLESGRID_HPP_
#include <fstream>
#include <set>
#include <list>
#include <numeric>
#include <map>
class CirclesGridClusterFinder
{
CirclesGridClusterFinder& operator=(const CirclesGridClusterFinder&);
CirclesGridClusterFinder(const CirclesGridClusterFinder&);
public:
CirclesGridClusterFinder(const cv::CirclesGridFinderParameters &parameters)
{
isAsymmetricGrid = parameters.gridType == cv::CirclesGridFinderParameters::ASYMMETRIC_GRID;
squareSize = parameters.squareSize;
maxRectifiedDistance = parameters.maxRectifiedDistance;
}
void findGrid(const std::vector<cv::Point2f> &points, cv::Size patternSize, std::vector<cv::Point2f>& centers);
//cluster 2d points by geometric coordinates
void hierarchicalClustering(const std::vector<cv::Point2f> &points, const cv::Size &patternSize, std::vector<cv::Point2f> &patternPoints);
private:
void findCorners(const std::vector<cv::Point2f> &hull2f, std::vector<cv::Point2f> &corners);
void findOutsideCorners(const std::vector<cv::Point2f> &corners, std::vector<cv::Point2f> &outsideCorners);
void getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners);
void rectifyPatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &sortedCorners, std::vector<cv::Point2f> &rectifiedPatternPoints);
void parsePatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &rectifiedPatternPoints, std::vector<cv::Point2f> &centers);
float squareSize, maxRectifiedDistance;
bool isAsymmetricGrid;
cv::Size patternSize;
};
class Graph
{
public:
typedef std::set<size_t> Neighbors;
struct Vertex
{
Neighbors neighbors;
};
typedef std::map<size_t, Vertex> Vertices;
Graph(size_t n);
void addVertex(size_t id);
void addEdge(size_t id1, size_t id2);
void removeEdge(size_t id1, size_t id2);
bool doesVertexExist(size_t id) const;
bool areVerticesAdjacent(size_t id1, size_t id2) const;
size_t getVerticesCount() const;
size_t getDegree(size_t id) const;
const Neighbors& getNeighbors(size_t id) const;
void floydWarshall(cv::Mat &distanceMatrix, int infinity = -1) const;
private:
Vertices vertices;
};
struct Path
{
int firstVertex;
int lastVertex;
int length;
std::vector<size_t> vertices;
Path(int first = -1, int last = -1, int len = -1)
{
firstVertex = first;
lastVertex = last;
length = len;
}
};
class CirclesGridFinder
{
public:
CirclesGridFinder(cv::Size patternSize, const std::vector<cv::Point2f> &testKeypoints,
const cv::CirclesGridFinderParameters &parameters = cv::CirclesGridFinderParameters());
bool findHoles();
static cv::Mat rectifyGrid(cv::Size detectedGridSize, const std::vector<cv::Point2f>& centers, const std::vector<
cv::Point2f> &keypoint, std::vector<cv::Point2f> &warpedKeypoints);
void getHoles(std::vector<cv::Point2f> &holes) const;
void getAsymmetricHoles(std::vector<cv::Point2f> &holes) const;
cv::Size getDetectedGridSize() const;
void drawBasis(const std::vector<cv::Point2f> &basis, cv::Point2f origin, cv::Mat &drawImg) const;
void drawBasisGraphs(const std::vector<Graph> &basisGraphs, cv::Mat &drawImg, bool drawEdges = true,
bool drawVertices = true) const;
void drawHoles(const cv::Mat &srcImage, cv::Mat &drawImage) const;
private:
void computeRNG(Graph &rng, std::vector<cv::Point2f> &vectors, cv::Mat *drawImage = 0) const;
void rng2gridGraph(Graph &rng, std::vector<cv::Point2f> &vectors) const;
void eraseUsedGraph(std::vector<Graph> &basisGraphs) const;
void filterOutliersByDensity(const std::vector<cv::Point2f> &samples, std::vector<cv::Point2f> &filteredSamples);
void findBasis(const std::vector<cv::Point2f> &samples, std::vector<cv::Point2f> &basis,
std::vector<Graph> &basisGraphs);
void findMCS(const std::vector<cv::Point2f> &basis, std::vector<Graph> &basisGraphs);
size_t findLongestPath(std::vector<Graph> &basisGraphs, Path &bestPath);
float computeGraphConfidence(const std::vector<Graph> &basisGraphs, bool addRow, const std::vector<size_t> &points,
const std::vector<size_t> &seeds);
void addHolesByGraph(const std::vector<Graph> &basisGraphs, bool addRow, cv::Point2f basisVec);
size_t findNearestKeypoint(cv::Point2f pt) const;
void addPoint(cv::Point2f pt, std::vector<size_t> &points);
void findCandidateLine(std::vector<size_t> &line, size_t seedLineIdx, bool addRow, cv::Point2f basisVec, std::vector<
size_t> &seeds);
void findCandidateHoles(std::vector<size_t> &above, std::vector<size_t> &below, bool addRow, cv::Point2f basisVec,
std::vector<size_t> &aboveSeeds, std::vector<size_t> &belowSeeds);
static bool areCentersNew(const std::vector<size_t> &newCenters, const std::vector<std::vector<size_t> > &holes);
bool isDetectionCorrect();
static void insertWinner(float aboveConfidence, float belowConfidence, float minConfidence, bool addRow,
const std::vector<size_t> &above, const std::vector<size_t> &below, std::vector<std::vector<
size_t> > &holes);
struct Segment
{
cv::Point2f s;
cv::Point2f e;
Segment(cv::Point2f _s, cv::Point2f _e);
};
//if endpoint is on a segment then function return false
static bool areSegmentsIntersecting(Segment seg1, Segment seg2);
static bool doesIntersectionExist(const std::vector<Segment> &corner, const std::vector<std::vector<Segment> > &segments);
void getCornerSegments(const std::vector<std::vector<size_t> > &points, std::vector<std::vector<Segment> > &segments,
std::vector<cv::Point> &cornerIndices, std::vector<cv::Point> &firstSteps,
std::vector<cv::Point> &secondSteps) const;
size_t getFirstCorner(std::vector<cv::Point> &largeCornerIndices, std::vector<cv::Point> &smallCornerIndices,
std::vector<cv::Point> &firstSteps, std::vector<cv::Point> &secondSteps) const;
static double getDirection(cv::Point2f p1, cv::Point2f p2, cv::Point2f p3);
std::vector<cv::Point2f> keypoints;
std::vector<std::vector<size_t> > holes;
std::vector<std::vector<size_t> > holes2;
std::vector<std::vector<size_t> > *largeHoles;
std::vector<std::vector<size_t> > *smallHoles;
const cv::Size_<size_t> patternSize;
cv::CirclesGridFinderParameters parameters;
CirclesGridFinder& operator=(const CirclesGridFinder&);
CirclesGridFinder(const CirclesGridFinder&);
};
#endif /* CIRCLESGRID_HPP_ */

471
Lib/opencv/sources/modules/calib3d/src/compat_ptsetreg.cpp Normal file
View File

@@ -0,0 +1,471 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
#include "calib3d_c_api.h"
/************************************************************************************\
Some backward compatibility stuff, to be moved to legacy or compat module
\************************************************************************************/
using cv::Ptr;
////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
CvLevMarq::CvLevMarq()
{
lambdaLg10 = 0; state = DONE;
criteria = cvTermCriteria(0,0,0);
iters = 0;
completeSymmFlag = false;
errNorm = prevErrNorm = DBL_MAX;
solveMethod = cv::DECOMP_SVD;
}
CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{
init(nparams, nerrs, criteria0, _completeSymmFlag);
}
void CvLevMarq::clear()
{
mask.release();
prevParam.release();
param.release();
J.release();
err.release();
JtJ.release();
JtJN.release();
JtErr.release();
JtJV.release();
JtJW.release();
}
CvLevMarq::~CvLevMarq()
{
clear();
}
void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{
if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
clear();
mask.reset(cvCreateMat( nparams, 1, CV_8U ));
cvSet(mask, cvScalarAll(1));
prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));
param.reset(cvCreateMat( nparams, 1, CV_64F ));
JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));
JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));
if( nerrs > 0 )
{
J.reset(cvCreateMat( nerrs, nparams, CV_64F ));
err.reset(cvCreateMat( nerrs, 1, CV_64F ));
}
errNorm = prevErrNorm = DBL_MAX;
lambdaLg10 = -3;
criteria = criteria0;
if( criteria.type & CV_TERMCRIT_ITER )
criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
else
criteria.max_iter = 30;
if( criteria.type & CV_TERMCRIT_EPS )
criteria.epsilon = MAX(criteria.epsilon, 0);
else
criteria.epsilon = DBL_EPSILON;
state = STARTED;
iters = 0;
completeSymmFlag = _completeSymmFlag;
solveMethod = cv::DECOMP_SVD;
}
bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
{
matJ = _err = 0;
assert( !err.empty() );
if( state == DONE )
{
_param = param;
return false;
}
if( state == STARTED )
{
_param = param;
cvZero( J );
cvZero( err );
matJ = J;
_err = err;
state = CALC_J;
return true;
}
if( state == CALC_J )
{
cvMulTransposed( J, JtJ, 1 );
cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
cvCopy( param, prevParam );
step();
if( iters == 0 )
prevErrNorm = cvNorm(err, 0, CV_L2);
_param = param;
cvZero( err );
_err = err;
state = CHECK_ERR;
return true;
}
assert( state == CHECK_ERR );
errNorm = cvNorm( err, 0, CV_L2 );
if( errNorm > prevErrNorm )
{
if( ++lambdaLg10 <= 16 )
{
step();
_param = param;
cvZero( err );
_err = err;
state = CHECK_ERR;
return true;
}
}
lambdaLg10 = MAX(lambdaLg10-1, -16);
if( ++iters >= criteria.max_iter ||
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
{
_param = param;
state = DONE;
return true;
}
prevErrNorm = errNorm;
_param = param;
cvZero(J);
matJ = J;
_err = err;
state = CALC_J;
return true;
}
bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
{
CV_Assert( !err );
if( state == DONE )
{
_param = param;
return false;
}
if( state == STARTED )
{
_param = param;
cvZero( JtJ );
cvZero( JtErr );
errNorm = 0;
_JtJ = JtJ;
_JtErr = JtErr;
_errNorm = &errNorm;
state = CALC_J;
return true;
}
if( state == CALC_J )
{
cvCopy( param, prevParam );
step();
_param = param;
prevErrNorm = errNorm;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
assert( state == CHECK_ERR );
if( errNorm > prevErrNorm )
{
if( ++lambdaLg10 <= 16 )
{
step();
_param = param;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
}
lambdaLg10 = MAX(lambdaLg10-1, -16);
if( ++iters >= criteria.max_iter ||
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
{
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = DONE;
return false;
}
prevErrNorm = errNorm;
cvZero( JtJ );
cvZero( JtErr );
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = CALC_J;
return true;
}
namespace {
static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
const std::vector<uchar>& rows) {
int nonzeros_cols = cv::countNonZero(cols);
cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)cols.size(); i++)
{
if (cols[i])
{
src.col(i).copyTo(tmp.col(j++));
}
}
int nonzeros_rows = cv::countNonZero(rows);
dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)rows.size(); i++)
{
if (rows[i])
{
tmp.row(i).copyTo(dst.row(j++));
}
}
}
}
void CvLevMarq::step()
{
using namespace cv;
const double LOG10 = log(10.);
double lambda = exp(lambdaLg10*LOG10);
int nparams = param->rows;
Mat _JtJ = cvarrToMat(JtJ);
Mat _mask = cvarrToMat(mask);
int nparams_nz = countNonZero(_mask);
if(!JtJN || JtJN->rows != nparams_nz) {
// prevent re-allocation in every step
JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
}
Mat _JtJN = cvarrToMat(JtJN);
Mat _JtErr = cvarrToMat(JtJV);
Mat_<double> nonzero_param = cvarrToMat(JtJW);
subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
subMatrix(_JtJ, _JtJN, _mask, _mask);
if( !err )
completeSymm( _JtJN, completeSymmFlag );
_JtJN.diag() *= 1. + lambda;
solve(_JtJN, _JtErr, nonzero_param, solveMethod);
int j = 0;
for( int i = 0; i < nparams; i++ )
param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
}
CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
{
return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters);
}
CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method,
double ransacReprojThreshold, CvMat* _mask, int maxIters,
double confidence)
{
cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
if( src.channels() == 1 && (src.rows == 2 || src.rows == 3) && src.cols > 3 )
cv::transpose(src, src);
if( dst.channels() == 1 && (dst.rows == 2 || dst.rows == 3) && dst.cols > 3 )
cv::transpose(dst, dst);
if ( maxIters < 0 )
maxIters = 0;
if ( maxIters > 2000 )
maxIters = 2000;
if ( confidence < 0 )
confidence = 0;
if ( confidence > 1 )
confidence = 1;
const cv::Mat H = cv::cvarrToMat(__H), mask = cv::cvarrToMat(_mask);
cv::Mat H0 = cv::findHomography(src, dst, method, ransacReprojThreshold,
_mask ? cv::_OutputArray(mask) : cv::_OutputArray(), maxIters,
confidence);
if( H0.empty() )
{
cv::Mat Hz = cv::cvarrToMat(__H);
Hz.setTo(cv::Scalar::all(0));
return 0;
}
H0.convertTo(H, H.type());
return 1;
}
CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
CvMat* fmatrix, int method,
double param1, double param2, CvMat* _mask )
{
cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2);
if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 )
cv::transpose(m1, m1);
if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 )
cv::transpose(m2, m2);
const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask);
cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2,
_mask ? cv::_OutputArray(mask) : cv::_OutputArray());
if( FM0.empty() )
{
cv::Mat FM0z = cv::cvarrToMat(fmatrix);
FM0z.setTo(cv::Scalar::all(0));
return 0;
}
CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 );
cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows));
FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type());
return FM1.rows / 3;
}
CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
const CvMat* fmatrix, CvMat* _lines )
{
cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix);
cv::Mat lines = cv::cvarrToMat(_lines);
const cv::Mat lines0 = lines;
if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 )
cv::transpose(pt, pt);
cv::computeCorrespondEpilines(pt, pointImageID, fm, lines);
bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3;
lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows));
if( tflag )
{
CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows );
if( lines0.type() == lines.type() )
transpose( lines, lines0 );
else
{
transpose( lines, lines );
lines.convertTo( lines0, lines0.type() );
}
}
else
{
CV_Assert( lines.size() == lines0.size() );
if( lines.data != lines0.data )
lines.convertTo(lines0, lines0.type());
}
}
CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
{
cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
const cv::Mat dst0 = dst;
int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
if( src.channels() == 1 && src.cols > d0 )
cv::transpose(src, src);
int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
if( d0 == d1 )
src.copyTo(dst);
else if( d0 < d1 )
cv::convertPointsToHomogeneous(src, dst);
else
cv::convertPointsFromHomogeneous(src, dst);
bool tflag = dst0.channels() == 1 && dst0.cols > d1;
dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
if( tflag )
{
CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
if( dst0.type() == dst.type() )
transpose( dst, dst0 );
else
{
transpose( dst, dst );
dst.convertTo( dst0, dst0.type() );
}
}
else
{
CV_Assert( dst.size() == dst0.size() );
if( dst.data != dst0.data )
dst.convertTo(dst0, dst0.type());
}
}

123
Lib/opencv/sources/modules/calib3d/src/distortion_model.hpp Normal file
View File

@@ -0,0 +1,123 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef OPENCV_IMGPROC_DETAIL_DISTORTION_MODEL_HPP
#define OPENCV_IMGPROC_DETAIL_DISTORTION_MODEL_HPP
//! @cond IGNORED
namespace cv { namespace detail {
/**
Computes the matrix for the projection onto a tilted image sensor
\param tauX angular parameter rotation around x-axis
\param tauY angular parameter rotation around y-axis
\param matTilt if not NULL returns the matrix
\f[
\vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}((\tau_x, \tau_y)}
{0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)}
{0}{0}{1} R(\tau_x, \tau_y)
\f]
where
\f[
R(\tau_x, \tau_y) =
\vecthreethree{\cos(\tau_y)}{0}{-\sin(\tau_y)}{0}{1}{0}{\sin(\tau_y)}{0}{\cos(\tau_y)}
\vecthreethree{1}{0}{0}{0}{\cos(\tau_x)}{\sin(\tau_x)}{0}{-\sin(\tau_x)}{\cos(\tau_x)} =
\vecthreethree{\cos(\tau_y)}{\sin(\tau_y)\sin(\tau_x)}{-\sin(\tau_y)\cos(\tau_x)}
{0}{\cos(\tau_x)}{\sin(\tau_x)}
{\sin(\tau_y)}{-\cos(\tau_y)\sin(\tau_x)}{\cos(\tau_y)\cos(\tau_x)}.
\f]
\param dMatTiltdTauX if not NULL it returns the derivative of matTilt with
respect to \f$\tau_x\f$.
\param dMatTiltdTauY if not NULL it returns the derivative of matTilt with
respect to \f$\tau_y\f$.
\param invMatTilt if not NULL it returns the inverse of matTilt
**/
template <typename FLOAT>
void computeTiltProjectionMatrix(FLOAT tauX,
FLOAT tauY,
Matx<FLOAT, 3, 3>* matTilt = 0,
Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
Matx<FLOAT, 3, 3>* invMatTilt = 0)
{
FLOAT cTauX = cos(tauX);
FLOAT sTauX = sin(tauX);
FLOAT cTauY = cos(tauY);
FLOAT sTauY = sin(tauY);
Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
if (matTilt)
{
// Matrix for trapezoidal distortion of tilted image sensor
*matTilt = matProjZ * matRotXY;
}
if (dMatTiltdTauX)
{
// Derivative with respect to tauX
Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
*dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
}
if (dMatTiltdTauY)
{
// Derivative with respect to tauY
Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
*dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
}
if (invMatTilt)
{
FLOAT inv = 1./matRotXY(2,2);
Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
*invMatTilt = matRotXY.t()*invMatProjZ;
}
}
}} // namespace detail, cv
//! @endcond
#endif // OPENCV_IMGPROC_DETAIL_DISTORTION_MODEL_HPP

658
Lib/opencv/sources/modules/calib3d/src/dls.cpp Normal file
View File

@@ -0,0 +1,658 @@
#include "precomp.hpp"
#include "dls.h"
#include <iostream>
#ifdef HAVE_EIGEN
# if defined __GNUC__ && defined __APPLE__
# pragma GCC diagnostic ignored "-Wshadow"
# endif
# if defined(_MSC_VER)
# pragma warning(push)
# pragma warning(disable:4701) // potentially uninitialized local variable
# pragma warning(disable:4702) // unreachable code
# pragma warning(disable:4714) // const marked as __forceinline not inlined
# endif
# include <Eigen/Core>
# include <Eigen/Eigenvalues>
# if defined(_MSC_VER)
# pragma warning(pop)
# endif
# include "opencv2/core/eigen.hpp"
#endif
using namespace std;
dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints)
{
N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
p = cv::Mat(3, N, CV_64F);
z = cv::Mat(3, N, CV_64F);
mn = cv::Mat::zeros(3, 1, CV_64F);
cost__ = 9999;
f1coeff.resize(21);
f2coeff.resize(21);
f3coeff.resize(21);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<cv::Point3f, cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d, cv::Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<cv::Point3f, cv::Point2d>(opoints, ipoints);
else
init_points<cv::Point3d, cv::Point2f>(opoints, ipoints);
}
dls::~dls()
{
// TODO Auto-generated destructor stub
}
bool dls::compute_pose(cv::Mat& R, cv::Mat& t)
{
std::vector<cv::Mat> R_;
R_.push_back(rotx(CV_PI/2));
R_.push_back(roty(CV_PI/2));
R_.push_back(rotz(CV_PI/2));
// version that calls dls 3 times, to avoid Cayley singularity
for (int i = 0; i < 3; ++i)
{
// Make a random rotation
cv::Mat pp = R_[i] * ( p - cv::repeat(mn, 1, p.cols) );
// clear for new data
C_est_.clear();
t_est_.clear();
cost_.clear();
this->run_kernel(pp); // run dls_pnp()
// find global minimum
for (unsigned int j = 0; j < cost_.size(); ++j)
{
if( cost_[j] < cost__ )
{
t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn;
C_est__ = C_est_[j] * R_[i];
cost__ = cost_[j];
}
}
}
if(C_est__.cols > 0 && C_est__.rows > 0)
{
C_est__.copyTo(R);
t_est__.copyTo(t);
return true;
}
return false;
}
void dls::run_kernel(const cv::Mat& pp)
{
cv::Mat Mtilde(27, 27, CV_64F);
cv::Mat D = cv::Mat::zeros(9, 9, CV_64F);
build_coeff_matrix(pp, Mtilde, D);
cv::Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i;
compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i);
/*
* Now check the solutions
*/
// extract the optimal solutions from the eigen decomposition of the
// Multiplication matrix
cv::Mat sols = cv::Mat::zeros(3, 27, CV_64F);
std::vector<double> cost;
int count = 0;
for (int k = 0; k < 27; ++k)
{
// V(:,k) = V(:,k)/V(1,k);
cv::Mat V_kA = eigenvec_r.col(k); // 27x1
cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at<double>(0)); // 1x1
cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A'
cv::Mat( V_k.t()).copyTo( eigenvec_r.col(k) );
//if (imag(V(2,k)) == 0)
#ifdef HAVE_EIGEN
const double epsilon = 1e-4;
if( eigenval_i.at<double>(k,0) >= -epsilon && eigenval_i.at<double>(k,0) <= epsilon )
#endif
{
double stmp[3];
stmp[0] = eigenvec_r.at<double>(9, k);
stmp[1] = eigenvec_r.at<double>(3, k);
stmp[2] = eigenvec_r.at<double>(1, k);
cv::Mat H = Hessian(stmp);
cv::Mat eigenvalues, eigenvectors;
cv::eigen(H, eigenvalues, eigenvectors);
if(positive_eigenvalues(&eigenvalues))
{
// sols(:,i) = stmp;
cv::Mat stmp_mat(3, 1, CV_64F, &stmp);
stmp_mat.copyTo( sols.col(count) );
cv::Mat Cbar = cayley2rotbar(stmp_mat);
cv::Mat Cbarvec = Cbar.reshape(1,1).t();
// cost(i) = CbarVec' * D * CbarVec;
cv::Mat cost_mat = Cbarvec.t() * D * Cbarvec;
cost.push_back( cost_mat.at<double>(0) );
count++;
}
}
}
// extract solutions
sols = sols.clone().colRange(0, count);
std::vector<cv::Mat> C_est, t_est;
for (int j = 0; j < sols.cols; ++j)
{
// recover the optimal orientation
// C_est(:,:,j) = 1/(1 + sols(:,j)' * sols(:,j)) * cayley2rotbar(sols(:,j));
cv::Mat sols_j = sols.col(j);
double sols_mult = 1./(1.+cv::Mat( sols_j.t() * sols_j ).at<double>(0));
cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult);
C_est.push_back( C_est_j );
cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F);
cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F);
for (int i = 0; i < N; ++i)
{
cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
cv::Mat z_mul = z.col(i)*z.col(i).t();
A2 += eye - z_mul;
b2 += (z_mul - eye) * C_est_j * pp.col(i);
}
// recover the optimal translation
cv::Mat X2; cv::solve(A2, b2, X2); // A\B
t_est.push_back(X2);
}
// check that the points are infront of the center of perspectivity
for (int k = 0; k < sols.cols; ++k)
{
cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols);
cv::Mat cam_points_k = cam_points.row(2);
if(is_empty(&cam_points_k))
{
cv::Mat C_valid = C_est[k], t_valid = t_est[k];
double cost_valid = cost[k];
C_est_.push_back(C_valid);
t_est_.push_back(t_valid);
cost_.push_back(cost_valid);
}
}
}
void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
{
CV_Assert(!pp.empty() && N > 0);
cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
// build coeff matrix
// An intermediate matrix, the inverse of what is called "H" in the paper
// (see eq. 25)
cv::Mat H = cv::Mat::zeros(3, 3, CV_64F);
cv::Mat A = cv::Mat::zeros(3, 9, CV_64F);
cv::Mat pp_i(3, 1, CV_64F);
cv::Mat z_i(3, 1, CV_64F);
for (int i = 0; i < N; ++i)
{
z.col(i).copyTo(z_i);
A += ( z_i*z_i.t() - eye ) * LeftMultVec(pp.col(i));
}
H = eye.mul(N) - z * z.t();
// A\B
cv::solve(H, A, A, cv::DECOMP_NORMAL);
H.release();
cv::Mat ppi_A(3, 1, CV_64F);
for (int i = 0; i < N; ++i)
{
z.col(i).copyTo(z_i);
ppi_A = LeftMultVec(pp.col(i)) + A;
D += ppi_A.t() * ( eye - z_i*z_i.t() ) * ppi_A;
}
A.release();
// fill the coefficients
fill_coeff(&D);
// generate random samples
std::vector<double> u(5);
cv::randn(u, 0, 200);
cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u);
cv::Mat M2_1 = M2(cv::Range(0,27), cv::Range(0,27));
cv::Mat M2_2 = M2(cv::Range(0,27), cv::Range(27,120));
cv::Mat M2_3 = M2(cv::Range(27,120), cv::Range(27,120));
cv::Mat M2_4 = M2(cv::Range(27,120), cv::Range(0,27));
M2.release();
// A/B = B'\A'
cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5);
M2_2.release(); M2_3.release();
// construct the multiplication matrix via schur compliment of the Macaulay
// matrix
Mtilde = M2_1 - M2_5.t()*M2_4;
}
void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag)
{
#ifdef HAVE_EIGEN
Eigen::MatrixXd Mtilde_eig, zeros_eig;
cv::cv2eigen(Mtilde, Mtilde_eig);
cv::cv2eigen(cv::Mat::zeros(27, 27, CV_64F), zeros_eig);
Eigen::MatrixXcd Mtilde_eig_cmplx(27, 27);
Mtilde_eig_cmplx.real() = Mtilde_eig;
Mtilde_eig_cmplx.imag() = zeros_eig;
Eigen::ComplexEigenSolver<Eigen::MatrixXcd> ces;
ces.compute(Mtilde_eig_cmplx);
Eigen::MatrixXd eigval_real = ces.eigenvalues().real();
Eigen::MatrixXd eigval_imag = ces.eigenvalues().imag();
Eigen::MatrixXd eigvec_real = ces.eigenvectors().real();
Eigen::MatrixXd eigvec_imag = ces.eigenvectors().imag();
cv::eigen2cv(eigval_real, eigenval_real);
cv::eigen2cv(eigval_imag, eigenval_imag);
cv::eigen2cv(eigvec_real, eigenvec_real);
cv::eigen2cv(eigvec_imag, eigenvec_imag);
#else
EigenvalueDecomposition es(Mtilde);
eigenval_real = es.eigenvalues();
eigenvec_real = es.eigenvectors();
eigenval_imag = eigenvec_imag = cv::Mat();
#endif
}
void dls::fill_coeff(const cv::Mat * D_mat)
{
// TODO: shift D and coefficients one position to left
double D[10][10]; // put D_mat into array
for (int i = 0; i < D_mat->rows; ++i)
{
const double* Di = D_mat->ptr<double>(i);
for (int j = 0; j < D_mat->cols; ++j)
{
D[i+1][j+1] = Di[j];
}
}
// F1 COEFFICIENT
f1coeff[1] = 2*D[1][6] - 2*D[1][8] + 2*D[5][6] - 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] - 2*D[8][1] - 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // constant term
f1coeff[2] = 6*D[1][2] + 6*D[1][4] + 6*D[2][1] - 6*D[2][5] - 6*D[2][9] + 6*D[4][1] - 6*D[4][5] - 6*D[4][9] - 6*D[5][2] - 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1^2 * s2
f1coeff[3] = 4*D[1][7] - 4*D[1][3] + 8*D[2][6] - 8*D[2][8] - 4*D[3][1] + 4*D[3][5] + 4*D[3][9] + 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] + 8*D[6][4] + 4*D[7][1] - 4*D[7][5] - 4*D[7][9] - 8*D[8][2] - 8*D[8][4] + 4*D[9][3] - 4*D[9][7]; // s1 * s2
f1coeff[4] = 4*D[1][2] - 4*D[1][4] + 4*D[2][1] - 4*D[2][5] - 4*D[2][9] + 8*D[3][6] - 8*D[3][8] - 4*D[4][1] + 4*D[4][5] + 4*D[4][9] - 4*D[5][2] + 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] - 8*D[7][8] - 8*D[8][3] - 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; //s1 * s3
f1coeff[5] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s2 * s3
f1coeff[6] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s2^2 * s3
f1coeff[7] = 2*D[2][5] - 2*D[1][4] - 2*D[2][1] - 2*D[1][2] - 2*D[2][9] - 2*D[4][1] + 2*D[4][5] - 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; //s2^3
f1coeff[8] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1 * s3^2
f1coeff[9] = 4*D[1][1] - 4*D[5][5] - 4*D[5][9] + 8*D[6][6] - 8*D[6][8] - 8*D[8][6] + 8*D[8][8] - 4*D[9][5] - 4*D[9][9]; // s1
f1coeff[10] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s3
f1coeff[11] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2
f1coeff[12] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2 * s3^2
f1coeff[13] = 6*D[1][6] - 6*D[1][8] - 6*D[5][6] + 6*D[5][8] + 6*D[6][1] - 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] + 6*D[8][9] - 6*D[9][6] + 6*D[9][8]; // s1^2
f1coeff[14] = 2*D[1][8] - 2*D[1][6] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] - 4*D[3][4] - 4*D[4][3] - 4*D[4][7] - 2*D[5][6] + 2*D[5][8] - 2*D[6][1] - 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // s3^2
f1coeff[15] = 2*D[1][8] - 2*D[1][6] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] - 4*D[3][4] - 4*D[4][3] + 4*D[4][7] + 2*D[5][6] - 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] + 2*D[8][9] - 2*D[9][6] + 2*D[9][8]; // s2^2
f1coeff[16] = 2*D[3][9] - 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[1][3] - 2*D[5][3] - 2*D[5][7] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 2*D[9][3] + 2*D[9][7]; // s3^3
f1coeff[17] = 4*D[1][6] + 4*D[1][8] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] + 8*D[3][4] + 8*D[4][3] + 8*D[4][7] - 4*D[5][6] - 4*D[5][8] + 4*D[6][1] - 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] - 4*D[8][9] - 4*D[9][6] - 4*D[9][8]; // s1 * s2 * s3
f1coeff[18] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1 * s2^2
f1coeff[19] = 6*D[1][3] + 6*D[1][7] + 6*D[3][1] - 6*D[3][5] - 6*D[3][9] - 6*D[5][3] - 6*D[5][7] + 6*D[7][1] - 6*D[7][5] - 6*D[7][9] - 6*D[9][3] - 6*D[9][7]; // s1^2 * s3
f1coeff[20] = 4*D[1][1] - 4*D[1][5] - 4*D[1][9] - 4*D[5][1] + 4*D[5][5] + 4*D[5][9] - 4*D[9][1] + 4*D[9][5] + 4*D[9][9]; // s1^3
// F2 COEFFICIENT
f2coeff[1] = - 2*D[1][3] + 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] + 2*D[5][7] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 2*D[9][3] + 2*D[9][7]; // constant term
f2coeff[2] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1^2 * s2
f2coeff[3] = 4*D[1][8] - 4*D[1][6] - 8*D[2][3] + 8*D[2][7] - 8*D[3][2] - 8*D[3][4] - 8*D[4][3] + 8*D[4][7] + 4*D[5][6] - 4*D[5][8] - 4*D[6][1] + 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] + 4*D[8][9] - 4*D[9][6] + 4*D[9][8]; // s1 * s2
f2coeff[4] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s3
f2coeff[5] = 4*D[1][4] - 4*D[1][2] - 4*D[2][1] + 4*D[2][5] - 4*D[2][9] - 8*D[3][6] - 8*D[3][8] + 4*D[4][1] - 4*D[4][5] + 4*D[4][9] + 4*D[5][2] - 4*D[5][4] - 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] - 8*D[8][3] + 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; // s2 * s3
f2coeff[6] = 6*D[5][6] - 6*D[1][8] - 6*D[1][6] + 6*D[5][8] - 6*D[6][1] + 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] - 6*D[8][9] - 6*D[9][6] - 6*D[9][8]; // s2^2 * s3
f2coeff[7] = 4*D[1][1] - 4*D[1][5] + 4*D[1][9] - 4*D[5][1] + 4*D[5][5] - 4*D[5][9] + 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s2^3
f2coeff[8] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1 * s3^2
f2coeff[9] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1
f2coeff[10] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3
f2coeff[11] = 8*D[3][3] - 4*D[1][9] - 4*D[1][1] - 8*D[3][7] + 4*D[5][5] - 8*D[7][3] + 8*D[7][7] - 4*D[9][1] - 4*D[9][9]; // s2
f2coeff[12] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2 * s3^2
f2coeff[13] = 2*D[1][7] - 2*D[1][3] + 4*D[2][6] - 4*D[2][8] - 2*D[3][1] + 2*D[3][5] + 2*D[3][9] + 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] + 4*D[6][4] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 4*D[8][2] - 4*D[8][4] + 2*D[9][3] - 2*D[9][7]; // s1^2
f2coeff[14] = 2*D[1][3] - 2*D[1][7] + 4*D[2][6] + 4*D[2][8] + 2*D[3][1] + 2*D[3][5] - 2*D[3][9] - 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] - 4*D[6][4] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 4*D[8][2] - 4*D[8][4] - 2*D[9][3] + 2*D[9][7]; // s3^2
f2coeff[15] = 6*D[1][3] - 6*D[1][7] + 6*D[3][1] - 6*D[3][5] + 6*D[3][9] - 6*D[5][3] + 6*D[5][7] - 6*D[7][1] + 6*D[7][5] - 6*D[7][9] + 6*D[9][3] - 6*D[9][7]; // s2^2
f2coeff[16] = 2*D[6][9] - 2*D[1][8] - 2*D[5][6] - 2*D[5][8] - 2*D[6][1] - 2*D[6][5] - 2*D[1][6] - 2*D[8][1] - 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3^3
f2coeff[17] = 8*D[2][6] - 4*D[1][7] - 4*D[1][3] + 8*D[2][8] - 4*D[3][1] + 4*D[3][5] - 4*D[3][9] + 8*D[4][6] + 8*D[4][8] + 4*D[5][3] + 4*D[5][7] + 8*D[6][2] + 8*D[6][4] - 4*D[7][1] + 4*D[7][5] - 4*D[7][9] + 8*D[8][2] + 8*D[8][4] - 4*D[9][3] - 4*D[9][7]; // s1 * s2 * s3
f2coeff[18] = 6*D[2][5] - 6*D[1][4] - 6*D[2][1] - 6*D[1][2] - 6*D[2][9] - 6*D[4][1] + 6*D[4][5] - 6*D[4][9] + 6*D[5][2] + 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1 * s2^2
f2coeff[19] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2 * s3
f2coeff[20] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 2*D[4][1] - 2*D[4][5] - 2*D[4][9] - 2*D[5][2] - 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; // s1^3
// F3 COEFFICIENT
f3coeff[1] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 2*D[4][1] - 2*D[4][5] - 2*D[4][9] + 2*D[5][2] - 2*D[5][4] + 2*D[9][2] - 2*D[9][4]; // constant term
f3coeff[2] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2 * s2
f3coeff[3] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s2
f3coeff[4] = 4*D[1][8] - 4*D[1][6] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] - 8*D[3][4] - 8*D[4][3] - 8*D[4][7] - 4*D[5][6] + 4*D[5][8] - 4*D[6][1] - 4*D[6][5] + 4*D[6][9] + 8*D[7][2] - 8*D[7][4] + 4*D[8][1] + 4*D[8][5] - 4*D[8][9] + 4*D[9][6] - 4*D[9][8]; // s1 * s3
f3coeff[5] = 4*D[1][3] - 4*D[1][7] + 8*D[2][6] + 8*D[2][8] + 4*D[3][1] + 4*D[3][5] - 4*D[3][9] - 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] - 8*D[6][4] - 4*D[7][1] - 4*D[7][5] + 4*D[7][9] + 8*D[8][2] - 8*D[8][4] - 4*D[9][3] + 4*D[9][7]; // s2 * s3
f3coeff[6] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2^2 * s3
f3coeff[7] = 2*D[5][6] - 2*D[1][8] - 2*D[1][6] + 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] - 2*D[8][1] + 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s2^3
f3coeff[8] = 6*D[3][9] - 6*D[1][7] - 6*D[3][1] - 6*D[3][5] - 6*D[1][3] - 6*D[5][3] - 6*D[5][7] - 6*D[7][1] - 6*D[7][5] + 6*D[7][9] + 6*D[9][3] + 6*D[9][7]; // s1 * s3^2
f3coeff[9] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s1
f3coeff[10] = 8*D[2][2] - 4*D[1][5] - 4*D[1][1] - 8*D[2][4] - 8*D[4][2] + 8*D[4][4] - 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s3
f3coeff[11] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s2
f3coeff[12] = 6*D[6][9] - 6*D[1][8] - 6*D[5][6] - 6*D[5][8] - 6*D[6][1] - 6*D[6][5] - 6*D[1][6] - 6*D[8][1] - 6*D[8][5] + 6*D[8][9] + 6*D[9][6] + 6*D[9][8]; // s2 * s3^2
f3coeff[13] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 4*D[3][6] - 4*D[3][8] - 2*D[4][1] + 2*D[4][5] + 2*D[4][9] - 2*D[5][2] + 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] - 4*D[8][3] - 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s1^2
f3coeff[14] = 6*D[1][4] - 6*D[1][2] - 6*D[2][1] - 6*D[2][5] + 6*D[2][9] + 6*D[4][1] + 6*D[4][5] - 6*D[4][9] - 6*D[5][2] + 6*D[5][4] + 6*D[9][2] - 6*D[9][4]; // s3^2
f3coeff[15] = 2*D[1][4] - 2*D[1][2] - 2*D[2][1] + 2*D[2][5] - 2*D[2][9] - 4*D[3][6] - 4*D[3][8] + 2*D[4][1] - 2*D[4][5] + 2*D[4][9] + 2*D[5][2] - 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] - 4*D[8][3] + 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s2^2
f3coeff[16] = 4*D[1][1] + 4*D[1][5] - 4*D[1][9] + 4*D[5][1] + 4*D[5][5] - 4*D[5][9] - 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s3^3
f3coeff[17] = 4*D[2][9] - 4*D[1][4] - 4*D[2][1] - 4*D[2][5] - 4*D[1][2] + 8*D[3][6] + 8*D[3][8] - 4*D[4][1] - 4*D[4][5] + 4*D[4][9] - 4*D[5][2] - 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] + 8*D[8][3] + 8*D[8][7] + 4*D[9][2] + 4*D[9][4]; // s1 * s2 * s3
f3coeff[18] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s1 * s2^2
f3coeff[19] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1^2 * s3
f3coeff[20] = 2*D[1][3] + 2*D[1][7] + 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] - 2*D[5][7] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 2*D[9][3] - 2*D[9][7]; // s1^3
}
cv::Mat dls::LeftMultVec(const cv::Mat& v)
{
cv::Mat mat_ = cv::Mat::zeros(3, 9, CV_64F);
for (int i = 0; i < 3; ++i)
{
mat_.at<double>(i, 3*i + 0) = v.at<double>(0);
mat_.at<double>(i, 3*i + 1) = v.at<double>(1);
mat_.at<double>(i, 3*i + 2) = v.at<double>(2);
}
return mat_;
}
cv::Mat dls::cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b, const std::vector<double>& c, const std::vector<double>& u)
{
// TODO: input matrix pointer
// TODO: shift coefficients one position to left
cv::Mat M = cv::Mat::zeros(120, 120, CV_64F);
M.at<double>(0,0)=u[1]; M.at<double>(0,35)=a[1]; M.at<double>(0,83)=b[1]; M.at<double>(0,118)=c[1];
M.at<double>(1,0)=u[4]; M.at<double>(1,1)=u[1]; M.at<double>(1,34)=a[1]; M.at<double>(1,35)=a[10]; M.at<double>(1,54)=b[1]; M.at<double>(1,83)=b[10]; M.at<double>(1,99)=c[1]; M.at<double>(1,118)=c[10];
M.at<double>(2,1)=u[4]; M.at<double>(2,2)=u[1]; M.at<double>(2,34)=a[10]; M.at<double>(2,35)=a[14]; M.at<double>(2,51)=a[1]; M.at<double>(2,54)=b[10]; M.at<double>(2,65)=b[1]; M.at<double>(2,83)=b[14]; M.at<double>(2,89)=c[1]; M.at<double>(2,99)=c[10]; M.at<double>(2,118)=c[14];
M.at<double>(3,0)=u[3]; M.at<double>(3,3)=u[1]; M.at<double>(3,35)=a[11]; M.at<double>(3,49)=a[1]; M.at<double>(3,76)=b[1]; M.at<double>(3,83)=b[11]; M.at<double>(3,118)=c[11]; M.at<double>(3,119)=c[1];
M.at<double>(4,1)=u[3]; M.at<double>(4,3)=u[4]; M.at<double>(4,4)=u[1]; M.at<double>(4,34)=a[11]; M.at<double>(4,35)=a[5]; M.at<double>(4,43)=a[1]; M.at<double>(4,49)=a[10]; M.at<double>(4,54)=b[11]; M.at<double>(4,71)=b[1]; M.at<double>(4,76)=b[10]; M.at<double>(4,83)=b[5]; M.at<double>(4,99)=c[11]; M.at<double>(4,100)=c[1]; M.at<double>(4,118)=c[5]; M.at<double>(4,119)=c[10];
M.at<double>(5,2)=u[3]; M.at<double>(5,4)=u[4]; M.at<double>(5,5)=u[1]; M.at<double>(5,34)=a[5]; M.at<double>(5,35)=a[12]; M.at<double>(5,41)=a[1]; M.at<double>(5,43)=a[10]; M.at<double>(5,49)=a[14]; M.at<double>(5,51)=a[11]; M.at<double>(5,54)=b[5]; M.at<double>(5,62)=b[1]; M.at<double>(5,65)=b[11]; M.at<double>(5,71)=b[10]; M.at<double>(5,76)=b[14]; M.at<double>(5,83)=b[12]; M.at<double>(5,89)=c[11]; M.at<double>(5,99)=c[5]; M.at<double>(5,100)=c[10]; M.at<double>(5,111)=c[1]; M.at<double>(5,118)=c[12]; M.at<double>(5,119)=c[14];
M.at<double>(6,3)=u[3]; M.at<double>(6,6)=u[1]; M.at<double>(6,30)=a[1]; M.at<double>(6,35)=a[15]; M.at<double>(6,49)=a[11]; M.at<double>(6,75)=b[1]; M.at<double>(6,76)=b[11]; M.at<double>(6,83)=b[15]; M.at<double>(6,107)=c[1]; M.at<double>(6,118)=c[15]; M.at<double>(6,119)=c[11];
M.at<double>(7,4)=u[3]; M.at<double>(7,6)=u[4]; M.at<double>(7,7)=u[1]; M.at<double>(7,30)=a[10]; M.at<double>(7,34)=a[15]; M.at<double>(7,35)=a[6]; M.at<double>(7,43)=a[11]; M.at<double>(7,45)=a[1]; M.at<double>(7,49)=a[5]; M.at<double>(7,54)=b[15]; M.at<double>(7,63)=b[1]; M.at<double>(7,71)=b[11]; M.at<double>(7,75)=b[10]; M.at<double>(7,76)=b[5]; M.at<double>(7,83)=b[6]; M.at<double>(7,99)=c[15]; M.at<double>(7,100)=c[11]; M.at<double>(7,107)=c[10]; M.at<double>(7,112)=c[1]; M.at<double>(7,118)=c[6]; M.at<double>(7,119)=c[5];
M.at<double>(8,5)=u[3]; M.at<double>(8,7)=u[4]; M.at<double>(8,8)=u[1]; M.at<double>(8,30)=a[14]; M.at<double>(8,34)=a[6]; M.at<double>(8,41)=a[11]; M.at<double>(8,43)=a[5]; M.at<double>(8,45)=a[10]; M.at<double>(8,46)=a[1]; M.at<double>(8,49)=a[12]; M.at<double>(8,51)=a[15]; M.at<double>(8,54)=b[6]; M.at<double>(8,62)=b[11]; M.at<double>(8,63)=b[10]; M.at<double>(8,65)=b[15]; M.at<double>(8,66)=b[1]; M.at<double>(8,71)=b[5]; M.at<double>(8,75)=b[14]; M.at<double>(8,76)=b[12]; M.at<double>(8,89)=c[15]; M.at<double>(8,99)=c[6]; M.at<double>(8,100)=c[5]; M.at<double>(8,102)=c[1]; M.at<double>(8,107)=c[14]; M.at<double>(8,111)=c[11]; M.at<double>(8,112)=c[10]; M.at<double>(8,119)=c[12];
M.at<double>(9,0)=u[2]; M.at<double>(9,9)=u[1]; M.at<double>(9,35)=a[9]; M.at<double>(9,36)=a[1]; M.at<double>(9,83)=b[9]; M.at<double>(9,84)=b[1]; M.at<double>(9,88)=c[1]; M.at<double>(9,118)=c[9];
M.at<double>(10,1)=u[2]; M.at<double>(10,9)=u[4]; M.at<double>(10,10)=u[1]; M.at<double>(10,33)=a[1]; M.at<double>(10,34)=a[9]; M.at<double>(10,35)=a[4]; M.at<double>(10,36)=a[10]; M.at<double>(10,54)=b[9]; M.at<double>(10,59)=b[1]; M.at<double>(10,83)=b[4]; M.at<double>(10,84)=b[10]; M.at<double>(10,88)=c[10]; M.at<double>(10,99)=c[9]; M.at<double>(10,117)=c[1]; M.at<double>(10,118)=c[4];
M.at<double>(11,2)=u[2]; M.at<double>(11,10)=u[4]; M.at<double>(11,11)=u[1]; M.at<double>(11,28)=a[1]; M.at<double>(11,33)=a[10]; M.at<double>(11,34)=a[4]; M.at<double>(11,35)=a[8]; M.at<double>(11,36)=a[14]; M.at<double>(11,51)=a[9]; M.at<double>(11,54)=b[4]; M.at<double>(11,57)=b[1]; M.at<double>(11,59)=b[10]; M.at<double>(11,65)=b[9]; M.at<double>(11,83)=b[8]; M.at<double>(11,84)=b[14]; M.at<double>(11,88)=c[14]; M.at<double>(11,89)=c[9]; M.at<double>(11,99)=c[4]; M.at<double>(11,114)=c[1]; M.at<double>(11,117)=c[10]; M.at<double>(11,118)=c[8];
M.at<double>(12,3)=u[2]; M.at<double>(12,9)=u[3]; M.at<double>(12,12)=u[1]; M.at<double>(12,35)=a[3]; M.at<double>(12,36)=a[11]; M.at<double>(12,39)=a[1]; M.at<double>(12,49)=a[9]; M.at<double>(12,76)=b[9]; M.at<double>(12,79)=b[1]; M.at<double>(12,83)=b[3]; M.at<double>(12,84)=b[11]; M.at<double>(12,88)=c[11]; M.at<double>(12,96)=c[1]; M.at<double>(12,118)=c[3]; M.at<double>(12,119)=c[9];
M.at<double>(13,4)=u[2]; M.at<double>(13,10)=u[3]; M.at<double>(13,12)=u[4]; M.at<double>(13,13)=u[1]; M.at<double>(13,33)=a[11]; M.at<double>(13,34)=a[3]; M.at<double>(13,35)=a[17]; M.at<double>(13,36)=a[5]; M.at<double>(13,39)=a[10]; M.at<double>(13,43)=a[9]; M.at<double>(13,47)=a[1]; M.at<double>(13,49)=a[4]; M.at<double>(13,54)=b[3]; M.at<double>(13,59)=b[11]; M.at<double>(13,60)=b[1]; M.at<double>(13,71)=b[9]; M.at<double>(13,76)=b[4]; M.at<double>(13,79)=b[10]; M.at<double>(13,83)=b[17]; M.at<double>(13,84)=b[5]; M.at<double>(13,88)=c[5]; M.at<double>(13,90)=c[1]; M.at<double>(13,96)=c[10]; M.at<double>(13,99)=c[3]; M.at<double>(13,100)=c[9]; M.at<double>(13,117)=c[11]; M.at<double>(13,118)=c[17]; M.at<double>(13,119)=c[4];
M.at<double>(14,5)=u[2]; M.at<double>(14,11)=u[3]; M.at<double>(14,13)=u[4]; M.at<double>(14,14)=u[1]; M.at<double>(14,28)=a[11]; M.at<double>(14,33)=a[5]; M.at<double>(14,34)=a[17]; M.at<double>(14,36)=a[12]; M.at<double>(14,39)=a[14]; M.at<double>(14,41)=a[9]; M.at<double>(14,42)=a[1]; M.at<double>(14,43)=a[4]; M.at<double>(14,47)=a[10]; M.at<double>(14,49)=a[8]; M.at<double>(14,51)=a[3]; M.at<double>(14,54)=b[17]; M.at<double>(14,56)=b[1]; M.at<double>(14,57)=b[11]; M.at<double>(14,59)=b[5]; M.at<double>(14,60)=b[10]; M.at<double>(14,62)=b[9]; M.at<double>(14,65)=b[3]; M.at<double>(14,71)=b[4]; M.at<double>(14,76)=b[8]; M.at<double>(14,79)=b[14]; M.at<double>(14,84)=b[12]; M.at<double>(14,88)=c[12]; M.at<double>(14,89)=c[3]; M.at<double>(14,90)=c[10]; M.at<double>(14,96)=c[14]; M.at<double>(14,99)=c[17]; M.at<double>(14,100)=c[4]; M.at<double>(14,106)=c[1]; M.at<double>(14,111)=c[9]; M.at<double>(14,114)=c[11]; M.at<double>(14,117)=c[5]; M.at<double>(14,119)=c[8];
M.at<double>(15,6)=u[2]; M.at<double>(15,12)=u[3]; M.at<double>(15,15)=u[1]; M.at<double>(15,29)=a[1]; M.at<double>(15,30)=a[9]; M.at<double>(15,35)=a[18]; M.at<double>(15,36)=a[15]; M.at<double>(15,39)=a[11]; M.at<double>(15,49)=a[3]; M.at<double>(15,74)=b[1]; M.at<double>(15,75)=b[9]; M.at<double>(15,76)=b[3]; M.at<double>(15,79)=b[11]; M.at<double>(15,83)=b[18]; M.at<double>(15,84)=b[15]; M.at<double>(15,88)=c[15]; M.at<double>(15,94)=c[1]; M.at<double>(15,96)=c[11]; M.at<double>(15,107)=c[9]; M.at<double>(15,118)=c[18]; M.at<double>(15,119)=c[3];
M.at<double>(16,7)=u[2]; M.at<double>(16,13)=u[3]; M.at<double>(16,15)=u[4]; M.at<double>(16,16)=u[1]; M.at<double>(16,29)=a[10]; M.at<double>(16,30)=a[4]; M.at<double>(16,33)=a[15]; M.at<double>(16,34)=a[18]; M.at<double>(16,36)=a[6]; M.at<double>(16,39)=a[5]; M.at<double>(16,43)=a[3]; M.at<double>(16,44)=a[1]; M.at<double>(16,45)=a[9]; M.at<double>(16,47)=a[11]; M.at<double>(16,49)=a[17]; M.at<double>(16,54)=b[18]; M.at<double>(16,59)=b[15]; M.at<double>(16,60)=b[11]; M.at<double>(16,63)=b[9]; M.at<double>(16,68)=b[1]; M.at<double>(16,71)=b[3]; M.at<double>(16,74)=b[10]; M.at<double>(16,75)=b[4]; M.at<double>(16,76)=b[17]; M.at<double>(16,79)=b[5]; M.at<double>(16,84)=b[6]; M.at<double>(16,88)=c[6]; M.at<double>(16,90)=c[11]; M.at<double>(16,94)=c[10]; M.at<double>(16,96)=c[5]; M.at<double>(16,97)=c[1]; M.at<double>(16,99)=c[18]; M.at<double>(16,100)=c[3]; M.at<double>(16,107)=c[4]; M.at<double>(16,112)=c[9]; M.at<double>(16,117)=c[15]; M.at<double>(16,119)=c[17];
M.at<double>(17,8)=u[2]; M.at<double>(17,14)=u[3]; M.at<double>(17,16)=u[4]; M.at<double>(17,17)=u[1]; M.at<double>(17,28)=a[15]; M.at<double>(17,29)=a[14]; M.at<double>(17,30)=a[8]; M.at<double>(17,33)=a[6]; M.at<double>(17,39)=a[12]; M.at<double>(17,41)=a[3]; M.at<double>(17,42)=a[11]; M.at<double>(17,43)=a[17]; M.at<double>(17,44)=a[10]; M.at<double>(17,45)=a[4]; M.at<double>(17,46)=a[9]; M.at<double>(17,47)=a[5]; M.at<double>(17,51)=a[18]; M.at<double>(17,56)=b[11]; M.at<double>(17,57)=b[15]; M.at<double>(17,59)=b[6]; M.at<double>(17,60)=b[5]; M.at<double>(17,62)=b[3]; M.at<double>(17,63)=b[4]; M.at<double>(17,65)=b[18]; M.at<double>(17,66)=b[9]; M.at<double>(17,68)=b[10]; M.at<double>(17,71)=b[17]; M.at<double>(17,74)=b[14]; M.at<double>(17,75)=b[8]; M.at<double>(17,79)=b[12]; M.at<double>(17,89)=c[18]; M.at<double>(17,90)=c[5]; M.at<double>(17,94)=c[14]; M.at<double>(17,96)=c[12]; M.at<double>(17,97)=c[10]; M.at<double>(17,100)=c[17]; M.at<double>(17,102)=c[9]; M.at<double>(17,106)=c[11]; M.at<double>(17,107)=c[8]; M.at<double>(17,111)=c[3]; M.at<double>(17,112)=c[4]; M.at<double>(17,114)=c[15]; M.at<double>(17,117)=c[6];
M.at<double>(18,9)=u[2]; M.at<double>(18,18)=u[1]; M.at<double>(18,35)=a[13]; M.at<double>(18,36)=a[9]; M.at<double>(18,53)=a[1]; M.at<double>(18,82)=b[1]; M.at<double>(18,83)=b[13]; M.at<double>(18,84)=b[9]; M.at<double>(18,87)=c[1]; M.at<double>(18,88)=c[9]; M.at<double>(18,118)=c[13];
M.at<double>(19,10)=u[2]; M.at<double>(19,18)=u[4]; M.at<double>(19,19)=u[1]; M.at<double>(19,32)=a[1]; M.at<double>(19,33)=a[9]; M.at<double>(19,34)=a[13]; M.at<double>(19,35)=a[19]; M.at<double>(19,36)=a[4]; M.at<double>(19,53)=a[10]; M.at<double>(19,54)=b[13]; M.at<double>(19,59)=b[9]; M.at<double>(19,61)=b[1]; M.at<double>(19,82)=b[10]; M.at<double>(19,83)=b[19]; M.at<double>(19,84)=b[4]; M.at<double>(19,87)=c[10]; M.at<double>(19,88)=c[4]; M.at<double>(19,99)=c[13]; M.at<double>(19,116)=c[1]; M.at<double>(19,117)=c[9]; M.at<double>(19,118)=c[19];
M.at<double>(20,11)=u[2]; M.at<double>(20,19)=u[4]; M.at<double>(20,20)=u[1]; M.at<double>(20,27)=a[1]; M.at<double>(20,28)=a[9]; M.at<double>(20,32)=a[10]; M.at<double>(20,33)=a[4]; M.at<double>(20,34)=a[19]; M.at<double>(20,36)=a[8]; M.at<double>(20,51)=a[13]; M.at<double>(20,53)=a[14]; M.at<double>(20,54)=b[19]; M.at<double>(20,55)=b[1]; M.at<double>(20,57)=b[9]; M.at<double>(20,59)=b[4]; M.at<double>(20,61)=b[10]; M.at<double>(20,65)=b[13]; M.at<double>(20,82)=b[14]; M.at<double>(20,84)=b[8]; M.at<double>(20,87)=c[14]; M.at<double>(20,88)=c[8]; M.at<double>(20,89)=c[13]; M.at<double>(20,99)=c[19]; M.at<double>(20,113)=c[1]; M.at<double>(20,114)=c[9]; M.at<double>(20,116)=c[10]; M.at<double>(20,117)=c[4];
M.at<double>(21,12)=u[2]; M.at<double>(21,18)=u[3]; M.at<double>(21,21)=u[1]; M.at<double>(21,35)=a[2]; M.at<double>(21,36)=a[3]; M.at<double>(21,38)=a[1]; M.at<double>(21,39)=a[9]; M.at<double>(21,49)=a[13]; M.at<double>(21,53)=a[11]; M.at<double>(21,76)=b[13]; M.at<double>(21,78)=b[1]; M.at<double>(21,79)=b[9]; M.at<double>(21,82)=b[11]; M.at<double>(21,83)=b[2]; M.at<double>(21,84)=b[3]; M.at<double>(21,87)=c[11]; M.at<double>(21,88)=c[3]; M.at<double>(21,92)=c[1]; M.at<double>(21,96)=c[9]; M.at<double>(21,118)=c[2]; M.at<double>(21,119)=c[13];
M.at<double>(22,13)=u[2]; M.at<double>(22,19)=u[3]; M.at<double>(22,21)=u[4]; M.at<double>(22,22)=u[1]; M.at<double>(22,32)=a[11]; M.at<double>(22,33)=a[3]; M.at<double>(22,34)=a[2]; M.at<double>(22,36)=a[17]; M.at<double>(22,38)=a[10]; M.at<double>(22,39)=a[4]; M.at<double>(22,40)=a[1]; M.at<double>(22,43)=a[13]; M.at<double>(22,47)=a[9]; M.at<double>(22,49)=a[19]; M.at<double>(22,53)=a[5]; M.at<double>(22,54)=b[2]; M.at<double>(22,59)=b[3]; M.at<double>(22,60)=b[9]; M.at<double>(22,61)=b[11]; M.at<double>(22,71)=b[13]; M.at<double>(22,72)=b[1]; M.at<double>(22,76)=b[19]; M.at<double>(22,78)=b[10]; M.at<double>(22,79)=b[4]; M.at<double>(22,82)=b[5]; M.at<double>(22,84)=b[17]; M.at<double>(22,87)=c[5]; M.at<double>(22,88)=c[17]; M.at<double>(22,90)=c[9]; M.at<double>(22,92)=c[10]; M.at<double>(22,95)=c[1]; M.at<double>(22,96)=c[4]; M.at<double>(22,99)=c[2]; M.at<double>(22,100)=c[13]; M.at<double>(22,116)=c[11]; M.at<double>(22,117)=c[3]; M.at<double>(22,119)=c[19];
M.at<double>(23,14)=u[2]; M.at<double>(23,20)=u[3]; M.at<double>(23,22)=u[4]; M.at<double>(23,23)=u[1]; M.at<double>(23,27)=a[11]; M.at<double>(23,28)=a[3]; M.at<double>(23,32)=a[5]; M.at<double>(23,33)=a[17]; M.at<double>(23,38)=a[14]; M.at<double>(23,39)=a[8]; M.at<double>(23,40)=a[10]; M.at<double>(23,41)=a[13]; M.at<double>(23,42)=a[9]; M.at<double>(23,43)=a[19]; M.at<double>(23,47)=a[4]; M.at<double>(23,51)=a[2]; M.at<double>(23,53)=a[12]; M.at<double>(23,55)=b[11]; M.at<double>(23,56)=b[9]; M.at<double>(23,57)=b[3]; M.at<double>(23,59)=b[17]; M.at<double>(23,60)=b[4]; M.at<double>(23,61)=b[5]; M.at<double>(23,62)=b[13]; M.at<double>(23,65)=b[2]; M.at<double>(23,71)=b[19]; M.at<double>(23,72)=b[10]; M.at<double>(23,78)=b[14]; M.at<double>(23,79)=b[8]; M.at<double>(23,82)=b[12]; M.at<double>(23,87)=c[12]; M.at<double>(23,89)=c[2]; M.at<double>(23,90)=c[4]; M.at<double>(23,92)=c[14]; M.at<double>(23,95)=c[10]; M.at<double>(23,96)=c[8]; M.at<double>(23,100)=c[19]; M.at<double>(23,106)=c[9]; M.at<double>(23,111)=c[13]; M.at<double>(23,113)=c[11]; M.at<double>(23,114)=c[3]; M.at<double>(23,116)=c[5]; M.at<double>(23,117)=c[17];
M.at<double>(24,15)=u[2]; M.at<double>(24,21)=u[3]; M.at<double>(24,24)=u[1]; M.at<double>(24,29)=a[9]; M.at<double>(24,30)=a[13]; M.at<double>(24,36)=a[18]; M.at<double>(24,38)=a[11]; M.at<double>(24,39)=a[3]; M.at<double>(24,49)=a[2]; M.at<double>(24,52)=a[1]; M.at<double>(24,53)=a[15]; M.at<double>(24,73)=b[1]; M.at<double>(24,74)=b[9]; M.at<double>(24,75)=b[13]; M.at<double>(24,76)=b[2]; M.at<double>(24,78)=b[11]; M.at<double>(24,79)=b[3]; M.at<double>(24,82)=b[15]; M.at<double>(24,84)=b[18]; M.at<double>(24,87)=c[15]; M.at<double>(24,88)=c[18]; M.at<double>(24,92)=c[11]; M.at<double>(24,93)=c[1]; M.at<double>(24,94)=c[9]; M.at<double>(24,96)=c[3]; M.at<double>(24,107)=c[13]; M.at<double>(24,119)=c[2];
M.at<double>(25,16)=u[2]; M.at<double>(25,22)=u[3]; M.at<double>(25,24)=u[4]; M.at<double>(25,25)=u[1]; M.at<double>(25,29)=a[4]; M.at<double>(25,30)=a[19]; M.at<double>(25,32)=a[15]; M.at<double>(25,33)=a[18]; M.at<double>(25,38)=a[5]; M.at<double>(25,39)=a[17]; M.at<double>(25,40)=a[11]; M.at<double>(25,43)=a[2]; M.at<double>(25,44)=a[9]; M.at<double>(25,45)=a[13]; M.at<double>(25,47)=a[3]; M.at<double>(25,52)=a[10]; M.at<double>(25,53)=a[6]; M.at<double>(25,59)=b[18]; M.at<double>(25,60)=b[3]; M.at<double>(25,61)=b[15]; M.at<double>(25,63)=b[13]; M.at<double>(25,68)=b[9]; M.at<double>(25,71)=b[2]; M.at<double>(25,72)=b[11]; M.at<double>(25,73)=b[10]; M.at<double>(25,74)=b[4]; M.at<double>(25,75)=b[19]; M.at<double>(25,78)=b[5]; M.at<double>(25,79)=b[17]; M.at<double>(25,82)=b[6]; M.at<double>(25,87)=c[6]; M.at<double>(25,90)=c[3]; M.at<double>(25,92)=c[5]; M.at<double>(25,93)=c[10]; M.at<double>(25,94)=c[4]; M.at<double>(25,95)=c[11]; M.at<double>(25,96)=c[17]; M.at<double>(25,97)=c[9]; M.at<double>(25,100)=c[2]; M.at<double>(25,107)=c[19]; M.at<double>(25,112)=c[13]; M.at<double>(25,116)=c[15]; M.at<double>(25,117)=c[18];
M.at<double>(26,17)=u[2]; M.at<double>(26,23)=u[3]; M.at<double>(26,25)=u[4]; M.at<double>(26,26)=u[1]; M.at<double>(26,27)=a[15]; M.at<double>(26,28)=a[18]; M.at<double>(26,29)=a[8]; M.at<double>(26,32)=a[6]; M.at<double>(26,38)=a[12]; M.at<double>(26,40)=a[5]; M.at<double>(26,41)=a[2]; M.at<double>(26,42)=a[3]; M.at<double>(26,44)=a[4]; M.at<double>(26,45)=a[19]; M.at<double>(26,46)=a[13]; M.at<double>(26,47)=a[17]; M.at<double>(26,52)=a[14]; M.at<double>(26,55)=b[15]; M.at<double>(26,56)=b[3]; M.at<double>(26,57)=b[18]; M.at<double>(26,60)=b[17]; M.at<double>(26,61)=b[6]; M.at<double>(26,62)=b[2]; M.at<double>(26,63)=b[19]; M.at<double>(26,66)=b[13]; M.at<double>(26,68)=b[4]; M.at<double>(26,72)=b[5]; M.at<double>(26,73)=b[14]; M.at<double>(26,74)=b[8]; M.at<double>(26,78)=b[12]; M.at<double>(26,90)=c[17]; M.at<double>(26,92)=c[12]; M.at<double>(26,93)=c[14]; M.at<double>(26,94)=c[8]; M.at<double>(26,95)=c[5]; M.at<double>(26,97)=c[4]; M.at<double>(26,102)=c[13]; M.at<double>(26,106)=c[3]; M.at<double>(26,111)=c[2]; M.at<double>(26,112)=c[19]; M.at<double>(26,113)=c[15]; M.at<double>(26,114)=c[18]; M.at<double>(26,116)=c[6];
M.at<double>(27,15)=u[3]; M.at<double>(27,29)=a[11]; M.at<double>(27,30)=a[3]; M.at<double>(27,36)=a[7]; M.at<double>(27,39)=a[15]; M.at<double>(27,49)=a[18]; M.at<double>(27,69)=b[9]; M.at<double>(27,70)=b[1]; M.at<double>(27,74)=b[11]; M.at<double>(27,75)=b[3]; M.at<double>(27,76)=b[18]; M.at<double>(27,79)=b[15]; M.at<double>(27,84)=b[7]; M.at<double>(27,88)=c[7]; M.at<double>(27,91)=c[1]; M.at<double>(27,94)=c[11]; M.at<double>(27,96)=c[15]; M.at<double>(27,107)=c[3]; M.at<double>(27,110)=c[9]; M.at<double>(27,119)=c[18];
M.at<double>(28,6)=u[3]; M.at<double>(28,30)=a[11]; M.at<double>(28,35)=a[7]; M.at<double>(28,49)=a[15]; M.at<double>(28,69)=b[1]; M.at<double>(28,75)=b[11]; M.at<double>(28,76)=b[15]; M.at<double>(28,83)=b[7]; M.at<double>(28,107)=c[11]; M.at<double>(28,110)=c[1]; M.at<double>(28,118)=c[7]; M.at<double>(28,119)=c[15];
M.at<double>(29,24)=u[3]; M.at<double>(29,29)=a[3]; M.at<double>(29,30)=a[2]; M.at<double>(29,38)=a[15]; M.at<double>(29,39)=a[18]; M.at<double>(29,52)=a[11]; M.at<double>(29,53)=a[7]; M.at<double>(29,69)=b[13]; M.at<double>(29,70)=b[9]; M.at<double>(29,73)=b[11]; M.at<double>(29,74)=b[3]; M.at<double>(29,75)=b[2]; M.at<double>(29,78)=b[15]; M.at<double>(29,79)=b[18]; M.at<double>(29,82)=b[7]; M.at<double>(29,87)=c[7]; M.at<double>(29,91)=c[9]; M.at<double>(29,92)=c[15]; M.at<double>(29,93)=c[11]; M.at<double>(29,94)=c[3]; M.at<double>(29,96)=c[18]; M.at<double>(29,107)=c[2]; M.at<double>(29,110)=c[13];
M.at<double>(30,37)=a[18]; M.at<double>(30,48)=a[7]; M.at<double>(30,52)=a[2]; M.at<double>(30,70)=b[20]; M.at<double>(30,73)=b[2]; M.at<double>(30,77)=b[18]; M.at<double>(30,81)=b[7]; M.at<double>(30,85)=c[7]; M.at<double>(30,91)=c[20]; M.at<double>(30,93)=c[2]; M.at<double>(30,98)=c[18];
M.at<double>(31,29)=a[2]; M.at<double>(31,37)=a[15]; M.at<double>(31,38)=a[18]; M.at<double>(31,50)=a[7]; M.at<double>(31,52)=a[3]; M.at<double>(31,69)=b[20]; M.at<double>(31,70)=b[13]; M.at<double>(31,73)=b[3]; M.at<double>(31,74)=b[2]; M.at<double>(31,77)=b[15]; M.at<double>(31,78)=b[18]; M.at<double>(31,80)=b[7]; M.at<double>(31,86)=c[7]; M.at<double>(31,91)=c[13]; M.at<double>(31,92)=c[18]; M.at<double>(31,93)=c[3]; M.at<double>(31,94)=c[2]; M.at<double>(31,98)=c[15]; M.at<double>(31,110)=c[20];
M.at<double>(32,48)=a[9]; M.at<double>(32,50)=a[13]; M.at<double>(32,53)=a[20]; M.at<double>(32,80)=b[13]; M.at<double>(32,81)=b[9]; M.at<double>(32,82)=b[20]; M.at<double>(32,85)=c[9]; M.at<double>(32,86)=c[13]; M.at<double>(32,87)=c[20];
M.at<double>(33,29)=a[15]; M.at<double>(33,30)=a[18]; M.at<double>(33,39)=a[7]; M.at<double>(33,64)=b[9]; M.at<double>(33,69)=b[3]; M.at<double>(33,70)=b[11]; M.at<double>(33,74)=b[15]; M.at<double>(33,75)=b[18]; M.at<double>(33,79)=b[7]; M.at<double>(33,91)=c[11]; M.at<double>(33,94)=c[15]; M.at<double>(33,96)=c[7]; M.at<double>(33,103)=c[9]; M.at<double>(33,107)=c[18]; M.at<double>(33,110)=c[3];
M.at<double>(34,29)=a[18]; M.at<double>(34,38)=a[7]; M.at<double>(34,52)=a[15]; M.at<double>(34,64)=b[13]; M.at<double>(34,69)=b[2]; M.at<double>(34,70)=b[3]; M.at<double>(34,73)=b[15]; M.at<double>(34,74)=b[18]; M.at<double>(34,78)=b[7]; M.at<double>(34,91)=c[3]; M.at<double>(34,92)=c[7]; M.at<double>(34,93)=c[15]; M.at<double>(34,94)=c[18]; M.at<double>(34,103)=c[13]; M.at<double>(34,110)=c[2];
M.at<double>(35,37)=a[7]; M.at<double>(35,52)=a[18]; M.at<double>(35,64)=b[20]; M.at<double>(35,70)=b[2]; M.at<double>(35,73)=b[18]; M.at<double>(35,77)=b[7]; M.at<double>(35,91)=c[2]; M.at<double>(35,93)=c[18]; M.at<double>(35,98)=c[7]; M.at<double>(35,103)=c[20];
M.at<double>(36,5)=u[4]; M.at<double>(36,34)=a[12]; M.at<double>(36,41)=a[10]; M.at<double>(36,43)=a[14]; M.at<double>(36,49)=a[16]; M.at<double>(36,51)=a[5]; M.at<double>(36,54)=b[12]; M.at<double>(36,62)=b[10]; M.at<double>(36,65)=b[5]; M.at<double>(36,71)=b[14]; M.at<double>(36,76)=b[16]; M.at<double>(36,89)=c[5]; M.at<double>(36,99)=c[12]; M.at<double>(36,100)=c[14]; M.at<double>(36,101)=c[1]; M.at<double>(36,109)=c[11]; M.at<double>(36,111)=c[10]; M.at<double>(36,119)=c[16];
M.at<double>(37,2)=u[4]; M.at<double>(37,34)=a[14]; M.at<double>(37,35)=a[16]; M.at<double>(37,51)=a[10]; M.at<double>(37,54)=b[14]; M.at<double>(37,65)=b[10]; M.at<double>(37,83)=b[16]; M.at<double>(37,89)=c[10]; M.at<double>(37,99)=c[14]; M.at<double>(37,109)=c[1]; M.at<double>(37,118)=c[16];
M.at<double>(38,30)=a[15]; M.at<double>(38,49)=a[7]; M.at<double>(38,64)=b[1]; M.at<double>(38,69)=b[11]; M.at<double>(38,75)=b[15]; M.at<double>(38,76)=b[7]; M.at<double>(38,103)=c[1]; M.at<double>(38,107)=c[15]; M.at<double>(38,110)=c[11]; M.at<double>(38,119)=c[7];
M.at<double>(39,28)=a[14]; M.at<double>(39,33)=a[16]; M.at<double>(39,51)=a[8]; M.at<double>(39,57)=b[14]; M.at<double>(39,59)=b[16]; M.at<double>(39,65)=b[8]; M.at<double>(39,89)=c[8]; M.at<double>(39,105)=c[9]; M.at<double>(39,108)=c[10]; M.at<double>(39,109)=c[4]; M.at<double>(39,114)=c[14]; M.at<double>(39,117)=c[16];
M.at<double>(40,27)=a[14]; M.at<double>(40,28)=a[8]; M.at<double>(40,32)=a[16]; M.at<double>(40,55)=b[14]; M.at<double>(40,57)=b[8]; M.at<double>(40,61)=b[16]; M.at<double>(40,105)=c[13]; M.at<double>(40,108)=c[4]; M.at<double>(40,109)=c[19]; M.at<double>(40,113)=c[14]; M.at<double>(40,114)=c[8]; M.at<double>(40,116)=c[16];
M.at<double>(41,30)=a[7]; M.at<double>(41,64)=b[11]; M.at<double>(41,69)=b[15]; M.at<double>(41,75)=b[7]; M.at<double>(41,103)=c[11]; M.at<double>(41,107)=c[7]; M.at<double>(41,110)=c[15];
M.at<double>(42,27)=a[8]; M.at<double>(42,31)=a[16]; M.at<double>(42,55)=b[8]; M.at<double>(42,58)=b[16]; M.at<double>(42,105)=c[20]; M.at<double>(42,108)=c[19]; M.at<double>(42,113)=c[8]; M.at<double>(42,115)=c[16];
M.at<double>(43,29)=a[7]; M.at<double>(43,64)=b[3]; M.at<double>(43,69)=b[18]; M.at<double>(43,70)=b[15]; M.at<double>(43,74)=b[7]; M.at<double>(43,91)=c[15]; M.at<double>(43,94)=c[7]; M.at<double>(43,103)=c[3]; M.at<double>(43,110)=c[18];
M.at<double>(44,28)=a[16]; M.at<double>(44,57)=b[16]; M.at<double>(44,105)=c[4]; M.at<double>(44,108)=c[14]; M.at<double>(44,109)=c[8]; M.at<double>(44,114)=c[16];
M.at<double>(45,27)=a[16]; M.at<double>(45,55)=b[16]; M.at<double>(45,105)=c[19]; M.at<double>(45,108)=c[8]; M.at<double>(45,113)=c[16];
M.at<double>(46,52)=a[7]; M.at<double>(46,64)=b[2]; M.at<double>(46,70)=b[18]; M.at<double>(46,73)=b[7]; M.at<double>(46,91)=c[18]; M.at<double>(46,93)=c[7]; M.at<double>(46,103)=c[2];
M.at<double>(47,40)=a[7]; M.at<double>(47,44)=a[18]; M.at<double>(47,52)=a[6]; M.at<double>(47,64)=b[19]; M.at<double>(47,67)=b[2]; M.at<double>(47,68)=b[18]; M.at<double>(47,70)=b[17]; M.at<double>(47,72)=b[7]; M.at<double>(47,73)=b[6]; M.at<double>(47,91)=c[17]; M.at<double>(47,93)=c[6]; M.at<double>(47,95)=c[7]; M.at<double>(47,97)=c[18]; M.at<double>(47,103)=c[19]; M.at<double>(47,104)=c[2];
M.at<double>(48,30)=a[6]; M.at<double>(48,43)=a[7]; M.at<double>(48,45)=a[15]; M.at<double>(48,63)=b[15]; M.at<double>(48,64)=b[10]; M.at<double>(48,67)=b[11]; M.at<double>(48,69)=b[5]; M.at<double>(48,71)=b[7]; M.at<double>(48,75)=b[6]; M.at<double>(48,100)=c[7]; M.at<double>(48,103)=c[10]; M.at<double>(48,104)=c[11]; M.at<double>(48,107)=c[6]; M.at<double>(48,110)=c[5]; M.at<double>(48,112)=c[15];
M.at<double>(49,41)=a[12]; M.at<double>(49,45)=a[16]; M.at<double>(49,46)=a[14]; M.at<double>(49,62)=b[12]; M.at<double>(49,63)=b[16]; M.at<double>(49,66)=b[14]; M.at<double>(49,101)=c[5]; M.at<double>(49,102)=c[14]; M.at<double>(49,105)=c[15]; M.at<double>(49,109)=c[6]; M.at<double>(49,111)=c[12]; M.at<double>(49,112)=c[16];
M.at<double>(50,41)=a[16]; M.at<double>(50,62)=b[16]; M.at<double>(50,101)=c[14]; M.at<double>(50,105)=c[5]; M.at<double>(50,109)=c[12]; M.at<double>(50,111)=c[16];
M.at<double>(51,64)=b[18]; M.at<double>(51,70)=b[7]; M.at<double>(51,91)=c[7]; M.at<double>(51,103)=c[18];
M.at<double>(52,41)=a[6]; M.at<double>(52,45)=a[12]; M.at<double>(52,46)=a[5]; M.at<double>(52,62)=b[6]; M.at<double>(52,63)=b[12]; M.at<double>(52,66)=b[5]; M.at<double>(52,67)=b[14]; M.at<double>(52,69)=b[16]; M.at<double>(52,101)=c[15]; M.at<double>(52,102)=c[5]; M.at<double>(52,104)=c[14]; M.at<double>(52,109)=c[7]; M.at<double>(52,110)=c[16]; M.at<double>(52,111)=c[6]; M.at<double>(52,112)=c[12];
M.at<double>(53,64)=b[15]; M.at<double>(53,69)=b[7]; M.at<double>(53,103)=c[15]; M.at<double>(53,110)=c[7];
M.at<double>(54,105)=c[14]; M.at<double>(54,109)=c[16];
M.at<double>(55,44)=a[7]; M.at<double>(55,64)=b[17]; M.at<double>(55,67)=b[18]; M.at<double>(55,68)=b[7]; M.at<double>(55,70)=b[6]; M.at<double>(55,91)=c[6]; M.at<double>(55,97)=c[7]; M.at<double>(55,103)=c[17]; M.at<double>(55,104)=c[18];
M.at<double>(56,105)=c[8]; M.at<double>(56,108)=c[16];
M.at<double>(57,64)=b[6]; M.at<double>(57,67)=b[7]; M.at<double>(57,103)=c[6]; M.at<double>(57,104)=c[7];
M.at<double>(58,46)=a[7]; M.at<double>(58,64)=b[12]; M.at<double>(58,66)=b[7]; M.at<double>(58,67)=b[6]; M.at<double>(58,102)=c[7]; M.at<double>(58,103)=c[12]; M.at<double>(58,104)=c[6];
M.at<double>(59,8)=u[4]; M.at<double>(59,30)=a[16]; M.at<double>(59,41)=a[5]; M.at<double>(59,43)=a[12]; M.at<double>(59,45)=a[14]; M.at<double>(59,46)=a[10]; M.at<double>(59,51)=a[6]; M.at<double>(59,62)=b[5]; M.at<double>(59,63)=b[14]; M.at<double>(59,65)=b[6]; M.at<double>(59,66)=b[10]; M.at<double>(59,71)=b[12]; M.at<double>(59,75)=b[16]; M.at<double>(59,89)=c[6]; M.at<double>(59,100)=c[12]; M.at<double>(59,101)=c[11]; M.at<double>(59,102)=c[10]; M.at<double>(59,107)=c[16]; M.at<double>(59,109)=c[15]; M.at<double>(59,111)=c[5]; M.at<double>(59,112)=c[14];
M.at<double>(60,8)=u[3]; M.at<double>(60,30)=a[12]; M.at<double>(60,41)=a[15]; M.at<double>(60,43)=a[6]; M.at<double>(60,45)=a[5]; M.at<double>(60,46)=a[11]; M.at<double>(60,51)=a[7]; M.at<double>(60,62)=b[15]; M.at<double>(60,63)=b[5]; M.at<double>(60,65)=b[7]; M.at<double>(60,66)=b[11]; M.at<double>(60,67)=b[10]; M.at<double>(60,69)=b[14]; M.at<double>(60,71)=b[6]; M.at<double>(60,75)=b[12]; M.at<double>(60,89)=c[7]; M.at<double>(60,100)=c[6]; M.at<double>(60,102)=c[11]; M.at<double>(60,104)=c[10]; M.at<double>(60,107)=c[12]; M.at<double>(60,110)=c[14]; M.at<double>(60,111)=c[15]; M.at<double>(60,112)=c[5];
M.at<double>(61,42)=a[16]; M.at<double>(61,56)=b[16]; M.at<double>(61,101)=c[8]; M.at<double>(61,105)=c[17]; M.at<double>(61,106)=c[16]; M.at<double>(61,108)=c[12];
M.at<double>(62,64)=b[7]; M.at<double>(62,103)=c[7];
M.at<double>(63,105)=c[16];
M.at<double>(64,46)=a[12]; M.at<double>(64,66)=b[12]; M.at<double>(64,67)=b[16]; M.at<double>(64,101)=c[6]; M.at<double>(64,102)=c[12]; M.at<double>(64,104)=c[16]; M.at<double>(64,105)=c[7];
M.at<double>(65,46)=a[6]; M.at<double>(65,64)=b[16]; M.at<double>(65,66)=b[6]; M.at<double>(65,67)=b[12]; M.at<double>(65,101)=c[7]; M.at<double>(65,102)=c[6]; M.at<double>(65,103)=c[16]; M.at<double>(65,104)=c[12];
M.at<double>(66,46)=a[16]; M.at<double>(66,66)=b[16]; M.at<double>(66,101)=c[12]; M.at<double>(66,102)=c[16]; M.at<double>(66,105)=c[6];
M.at<double>(67,101)=c[16]; M.at<double>(67,105)=c[12];
M.at<double>(68,41)=a[14]; M.at<double>(68,43)=a[16]; M.at<double>(68,51)=a[12]; M.at<double>(68,62)=b[14]; M.at<double>(68,65)=b[12]; M.at<double>(68,71)=b[16]; M.at<double>(68,89)=c[12]; M.at<double>(68,100)=c[16]; M.at<double>(68,101)=c[10]; M.at<double>(68,105)=c[11]; M.at<double>(68,109)=c[5]; M.at<double>(68,111)=c[14];
M.at<double>(69,37)=a[2]; M.at<double>(69,48)=a[18]; M.at<double>(69,52)=a[20]; M.at<double>(69,73)=b[20]; M.at<double>(69,77)=b[2]; M.at<double>(69,81)=b[18]; M.at<double>(69,85)=c[18]; M.at<double>(69,93)=c[20]; M.at<double>(69,98)=c[2];
M.at<double>(70,20)=u[2]; M.at<double>(70,27)=a[9]; M.at<double>(70,28)=a[13]; M.at<double>(70,31)=a[10]; M.at<double>(70,32)=a[4]; M.at<double>(70,33)=a[19]; M.at<double>(70,50)=a[14]; M.at<double>(70,51)=a[20]; M.at<double>(70,53)=a[8]; M.at<double>(70,55)=b[9]; M.at<double>(70,57)=b[13]; M.at<double>(70,58)=b[10]; M.at<double>(70,59)=b[19]; M.at<double>(70,61)=b[4]; M.at<double>(70,65)=b[20]; M.at<double>(70,80)=b[14]; M.at<double>(70,82)=b[8]; M.at<double>(70,86)=c[14]; M.at<double>(70,87)=c[8]; M.at<double>(70,89)=c[20]; M.at<double>(70,113)=c[9]; M.at<double>(70,114)=c[13]; M.at<double>(70,115)=c[10]; M.at<double>(70,116)=c[4]; M.at<double>(70,117)=c[19];
M.at<double>(71,45)=a[7]; M.at<double>(71,63)=b[7]; M.at<double>(71,64)=b[5]; M.at<double>(71,67)=b[15]; M.at<double>(71,69)=b[6]; M.at<double>(71,103)=c[5]; M.at<double>(71,104)=c[15]; M.at<double>(71,110)=c[6]; M.at<double>(71,112)=c[7];
M.at<double>(72,41)=a[7]; M.at<double>(72,45)=a[6]; M.at<double>(72,46)=a[15]; M.at<double>(72,62)=b[7]; M.at<double>(72,63)=b[6]; M.at<double>(72,64)=b[14]; M.at<double>(72,66)=b[15]; M.at<double>(72,67)=b[5]; M.at<double>(72,69)=b[12]; M.at<double>(72,102)=c[15]; M.at<double>(72,103)=c[14]; M.at<double>(72,104)=c[5]; M.at<double>(72,110)=c[12]; M.at<double>(72,111)=c[7]; M.at<double>(72,112)=c[6];
M.at<double>(73,48)=a[13]; M.at<double>(73,50)=a[20]; M.at<double>(73,80)=b[20]; M.at<double>(73,81)=b[13]; M.at<double>(73,85)=c[13]; M.at<double>(73,86)=c[20];
M.at<double>(74,25)=u[3]; M.at<double>(74,29)=a[17]; M.at<double>(74,32)=a[7]; M.at<double>(74,38)=a[6]; M.at<double>(74,40)=a[15]; M.at<double>(74,44)=a[3]; M.at<double>(74,45)=a[2]; M.at<double>(74,47)=a[18]; M.at<double>(74,52)=a[5]; M.at<double>(74,60)=b[18]; M.at<double>(74,61)=b[7]; M.at<double>(74,63)=b[2]; M.at<double>(74,67)=b[13]; M.at<double>(74,68)=b[3]; M.at<double>(74,69)=b[19]; M.at<double>(74,70)=b[4]; M.at<double>(74,72)=b[15]; M.at<double>(74,73)=b[5]; M.at<double>(74,74)=b[17]; M.at<double>(74,78)=b[6]; M.at<double>(74,90)=c[18]; M.at<double>(74,91)=c[4]; M.at<double>(74,92)=c[6]; M.at<double>(74,93)=c[5]; M.at<double>(74,94)=c[17]; M.at<double>(74,95)=c[15]; M.at<double>(74,97)=c[3]; M.at<double>(74,104)=c[13]; M.at<double>(74,110)=c[19]; M.at<double>(74,112)=c[2]; M.at<double>(74,116)=c[7];
M.at<double>(75,21)=u[2]; M.at<double>(75,36)=a[2]; M.at<double>(75,37)=a[1]; M.at<double>(75,38)=a[9]; M.at<double>(75,39)=a[13]; M.at<double>(75,49)=a[20]; M.at<double>(75,50)=a[11]; M.at<double>(75,53)=a[3]; M.at<double>(75,76)=b[20]; M.at<double>(75,77)=b[1]; M.at<double>(75,78)=b[9]; M.at<double>(75,79)=b[13]; M.at<double>(75,80)=b[11]; M.at<double>(75,82)=b[3]; M.at<double>(75,84)=b[2]; M.at<double>(75,86)=c[11]; M.at<double>(75,87)=c[3]; M.at<double>(75,88)=c[2]; M.at<double>(75,92)=c[9]; M.at<double>(75,96)=c[13]; M.at<double>(75,98)=c[1]; M.at<double>(75,119)=c[20];
M.at<double>(76,48)=a[20]; M.at<double>(76,81)=b[20]; M.at<double>(76,85)=c[20];
M.at<double>(77,34)=a[16]; M.at<double>(77,51)=a[14]; M.at<double>(77,54)=b[16]; M.at<double>(77,65)=b[14]; M.at<double>(77,89)=c[14]; M.at<double>(77,99)=c[16]; M.at<double>(77,105)=c[1]; M.at<double>(77,109)=c[10];
M.at<double>(78,27)=a[17]; M.at<double>(78,31)=a[12]; M.at<double>(78,37)=a[16]; M.at<double>(78,40)=a[8]; M.at<double>(78,42)=a[19]; M.at<double>(78,55)=b[17]; M.at<double>(78,56)=b[19]; M.at<double>(78,58)=b[12]; M.at<double>(78,72)=b[8]; M.at<double>(78,77)=b[16]; M.at<double>(78,95)=c[8]; M.at<double>(78,98)=c[16]; M.at<double>(78,101)=c[20]; M.at<double>(78,106)=c[19]; M.at<double>(78,108)=c[2]; M.at<double>(78,113)=c[17]; M.at<double>(78,115)=c[12];
M.at<double>(79,42)=a[12]; M.at<double>(79,44)=a[16]; M.at<double>(79,46)=a[8]; M.at<double>(79,56)=b[12]; M.at<double>(79,66)=b[8]; M.at<double>(79,68)=b[16]; M.at<double>(79,97)=c[16]; M.at<double>(79,101)=c[17]; M.at<double>(79,102)=c[8]; M.at<double>(79,105)=c[18]; M.at<double>(79,106)=c[12]; M.at<double>(79,108)=c[6];
M.at<double>(80,14)=u[4]; M.at<double>(80,28)=a[5]; M.at<double>(80,33)=a[12]; M.at<double>(80,39)=a[16]; M.at<double>(80,41)=a[4]; M.at<double>(80,42)=a[10]; M.at<double>(80,43)=a[8]; M.at<double>(80,47)=a[14]; M.at<double>(80,51)=a[17]; M.at<double>(80,56)=b[10]; M.at<double>(80,57)=b[5]; M.at<double>(80,59)=b[12]; M.at<double>(80,60)=b[14]; M.at<double>(80,62)=b[4]; M.at<double>(80,65)=b[17]; M.at<double>(80,71)=b[8]; M.at<double>(80,79)=b[16]; M.at<double>(80,89)=c[17]; M.at<double>(80,90)=c[14]; M.at<double>(80,96)=c[16]; M.at<double>(80,100)=c[8]; M.at<double>(80,101)=c[9]; M.at<double>(80,106)=c[10]; M.at<double>(80,108)=c[11]; M.at<double>(80,109)=c[3]; M.at<double>(80,111)=c[4]; M.at<double>(80,114)=c[5]; M.at<double>(80,117)=c[12];
M.at<double>(81,31)=a[3]; M.at<double>(81,32)=a[2]; M.at<double>(81,37)=a[4]; M.at<double>(81,38)=a[19]; M.at<double>(81,40)=a[13]; M.at<double>(81,47)=a[20]; M.at<double>(81,48)=a[5]; M.at<double>(81,50)=a[17]; M.at<double>(81,58)=b[3]; M.at<double>(81,60)=b[20]; M.at<double>(81,61)=b[2]; M.at<double>(81,72)=b[13]; M.at<double>(81,77)=b[4]; M.at<double>(81,78)=b[19]; M.at<double>(81,80)=b[17]; M.at<double>(81,81)=b[5]; M.at<double>(81,85)=c[5]; M.at<double>(81,86)=c[17]; M.at<double>(81,90)=c[20]; M.at<double>(81,92)=c[19]; M.at<double>(81,95)=c[13]; M.at<double>(81,98)=c[4]; M.at<double>(81,115)=c[3]; M.at<double>(81,116)=c[2];
M.at<double>(82,29)=a[6]; M.at<double>(82,44)=a[15]; M.at<double>(82,45)=a[18]; M.at<double>(82,47)=a[7]; M.at<double>(82,60)=b[7]; M.at<double>(82,63)=b[18]; M.at<double>(82,64)=b[4]; M.at<double>(82,67)=b[3]; M.at<double>(82,68)=b[15]; M.at<double>(82,69)=b[17]; M.at<double>(82,70)=b[5]; M.at<double>(82,74)=b[6]; M.at<double>(82,90)=c[7]; M.at<double>(82,91)=c[5]; M.at<double>(82,94)=c[6]; M.at<double>(82,97)=c[15]; M.at<double>(82,103)=c[4]; M.at<double>(82,104)=c[3]; M.at<double>(82,110)=c[17]; M.at<double>(82,112)=c[18];
M.at<double>(83,26)=u[2]; M.at<double>(83,27)=a[18]; M.at<double>(83,31)=a[6]; M.at<double>(83,37)=a[12]; M.at<double>(83,40)=a[17]; M.at<double>(83,42)=a[2]; M.at<double>(83,44)=a[19]; M.at<double>(83,46)=a[20]; M.at<double>(83,52)=a[8]; M.at<double>(83,55)=b[18]; M.at<double>(83,56)=b[2]; M.at<double>(83,58)=b[6]; M.at<double>(83,66)=b[20]; M.at<double>(83,68)=b[19]; M.at<double>(83,72)=b[17]; M.at<double>(83,73)=b[8]; M.at<double>(83,77)=b[12]; M.at<double>(83,93)=c[8]; M.at<double>(83,95)=c[17]; M.at<double>(83,97)=c[19]; M.at<double>(83,98)=c[12]; M.at<double>(83,102)=c[20]; M.at<double>(83,106)=c[2]; M.at<double>(83,113)=c[18]; M.at<double>(83,115)=c[6];
M.at<double>(84,16)=u[3]; M.at<double>(84,29)=a[5]; M.at<double>(84,30)=a[17]; M.at<double>(84,33)=a[7]; M.at<double>(84,39)=a[6]; M.at<double>(84,43)=a[18]; M.at<double>(84,44)=a[11]; M.at<double>(84,45)=a[3]; M.at<double>(84,47)=a[15]; M.at<double>(84,59)=b[7]; M.at<double>(84,60)=b[15]; M.at<double>(84,63)=b[3]; M.at<double>(84,67)=b[9]; M.at<double>(84,68)=b[11]; M.at<double>(84,69)=b[4]; M.at<double>(84,70)=b[10]; M.at<double>(84,71)=b[18]; M.at<double>(84,74)=b[5]; M.at<double>(84,75)=b[17]; M.at<double>(84,79)=b[6]; M.at<double>(84,90)=c[15]; M.at<double>(84,91)=c[10]; M.at<double>(84,94)=c[5]; M.at<double>(84,96)=c[6]; M.at<double>(84,97)=c[11]; M.at<double>(84,100)=c[18]; M.at<double>(84,104)=c[9]; M.at<double>(84,107)=c[17]; M.at<double>(84,110)=c[4]; M.at<double>(84,112)=c[3]; M.at<double>(84,117)=c[7];
M.at<double>(85,25)=u[2]; M.at<double>(85,29)=a[19]; M.at<double>(85,31)=a[15]; M.at<double>(85,32)=a[18]; M.at<double>(85,37)=a[5]; M.at<double>(85,38)=a[17]; M.at<double>(85,40)=a[3]; M.at<double>(85,44)=a[13]; M.at<double>(85,45)=a[20]; M.at<double>(85,47)=a[2]; M.at<double>(85,50)=a[6]; M.at<double>(85,52)=a[4]; M.at<double>(85,58)=b[15]; M.at<double>(85,60)=b[2]; M.at<double>(85,61)=b[18]; M.at<double>(85,63)=b[20]; M.at<double>(85,68)=b[13]; M.at<double>(85,72)=b[3]; M.at<double>(85,73)=b[4]; M.at<double>(85,74)=b[19]; M.at<double>(85,77)=b[5]; M.at<double>(85,78)=b[17]; M.at<double>(85,80)=b[6]; M.at<double>(85,86)=c[6]; M.at<double>(85,90)=c[2]; M.at<double>(85,92)=c[17]; M.at<double>(85,93)=c[4]; M.at<double>(85,94)=c[19]; M.at<double>(85,95)=c[3]; M.at<double>(85,97)=c[13]; M.at<double>(85,98)=c[5]; M.at<double>(85,112)=c[20]; M.at<double>(85,115)=c[15]; M.at<double>(85,116)=c[18];
M.at<double>(86,31)=a[18]; M.at<double>(86,37)=a[17]; M.at<double>(86,40)=a[2]; M.at<double>(86,44)=a[20]; M.at<double>(86,48)=a[6]; M.at<double>(86,52)=a[19]; M.at<double>(86,58)=b[18]; M.at<double>(86,68)=b[20]; M.at<double>(86,72)=b[2]; M.at<double>(86,73)=b[19]; M.at<double>(86,77)=b[17]; M.at<double>(86,81)=b[6]; M.at<double>(86,85)=c[6]; M.at<double>(86,93)=c[19]; M.at<double>(86,95)=c[2]; M.at<double>(86,97)=c[20]; M.at<double>(86,98)=c[17]; M.at<double>(86,115)=c[18];
M.at<double>(87,22)=u[2]; M.at<double>(87,31)=a[11]; M.at<double>(87,32)=a[3]; M.at<double>(87,33)=a[2]; M.at<double>(87,37)=a[10]; M.at<double>(87,38)=a[4]; M.at<double>(87,39)=a[19]; M.at<double>(87,40)=a[9]; M.at<double>(87,43)=a[20]; M.at<double>(87,47)=a[13]; M.at<double>(87,50)=a[5]; M.at<double>(87,53)=a[17]; M.at<double>(87,58)=b[11]; M.at<double>(87,59)=b[2]; M.at<double>(87,60)=b[13]; M.at<double>(87,61)=b[3]; M.at<double>(87,71)=b[20]; M.at<double>(87,72)=b[9]; M.at<double>(87,77)=b[10]; M.at<double>(87,78)=b[4]; M.at<double>(87,79)=b[19]; M.at<double>(87,80)=b[5]; M.at<double>(87,82)=b[17]; M.at<double>(87,86)=c[5]; M.at<double>(87,87)=c[17]; M.at<double>(87,90)=c[13]; M.at<double>(87,92)=c[4]; M.at<double>(87,95)=c[9]; M.at<double>(87,96)=c[19]; M.at<double>(87,98)=c[10]; M.at<double>(87,100)=c[20]; M.at<double>(87,115)=c[11]; M.at<double>(87,116)=c[3]; M.at<double>(87,117)=c[2];
M.at<double>(88,27)=a[2]; M.at<double>(88,31)=a[17]; M.at<double>(88,37)=a[8]; M.at<double>(88,40)=a[19]; M.at<double>(88,42)=a[20]; M.at<double>(88,48)=a[12]; M.at<double>(88,55)=b[2]; M.at<double>(88,56)=b[20]; M.at<double>(88,58)=b[17]; M.at<double>(88,72)=b[19]; M.at<double>(88,77)=b[8]; M.at<double>(88,81)=b[12]; M.at<double>(88,85)=c[12]; M.at<double>(88,95)=c[19]; M.at<double>(88,98)=c[8]; M.at<double>(88,106)=c[20]; M.at<double>(88,113)=c[2]; M.at<double>(88,115)=c[17];
M.at<double>(89,31)=a[7]; M.at<double>(89,37)=a[6]; M.at<double>(89,40)=a[18]; M.at<double>(89,44)=a[2]; M.at<double>(89,52)=a[17]; M.at<double>(89,58)=b[7]; M.at<double>(89,67)=b[20]; M.at<double>(89,68)=b[2]; M.at<double>(89,70)=b[19]; M.at<double>(89,72)=b[18]; M.at<double>(89,73)=b[17]; M.at<double>(89,77)=b[6]; M.at<double>(89,91)=c[19]; M.at<double>(89,93)=c[17]; M.at<double>(89,95)=c[18]; M.at<double>(89,97)=c[2]; M.at<double>(89,98)=c[6]; M.at<double>(89,104)=c[20]; M.at<double>(89,115)=c[7];
M.at<double>(90,27)=a[12]; M.at<double>(90,40)=a[16]; M.at<double>(90,42)=a[8]; M.at<double>(90,55)=b[12]; M.at<double>(90,56)=b[8]; M.at<double>(90,72)=b[16]; M.at<double>(90,95)=c[16]; M.at<double>(90,101)=c[19]; M.at<double>(90,105)=c[2]; M.at<double>(90,106)=c[8]; M.at<double>(90,108)=c[17]; M.at<double>(90,113)=c[12];
M.at<double>(91,23)=u[2]; M.at<double>(91,27)=a[3]; M.at<double>(91,28)=a[2]; M.at<double>(91,31)=a[5]; M.at<double>(91,32)=a[17]; M.at<double>(91,37)=a[14]; M.at<double>(91,38)=a[8]; M.at<double>(91,40)=a[4]; M.at<double>(91,41)=a[20]; M.at<double>(91,42)=a[13]; M.at<double>(91,47)=a[19]; M.at<double>(91,50)=a[12]; M.at<double>(91,55)=b[3]; M.at<double>(91,56)=b[13]; M.at<double>(91,57)=b[2]; M.at<double>(91,58)=b[5]; M.at<double>(91,60)=b[19]; M.at<double>(91,61)=b[17]; M.at<double>(91,62)=b[20]; M.at<double>(91,72)=b[4]; M.at<double>(91,77)=b[14]; M.at<double>(91,78)=b[8]; M.at<double>(91,80)=b[12]; M.at<double>(91,86)=c[12]; M.at<double>(91,90)=c[19]; M.at<double>(91,92)=c[8]; M.at<double>(91,95)=c[4]; M.at<double>(91,98)=c[14]; M.at<double>(91,106)=c[13]; M.at<double>(91,111)=c[20]; M.at<double>(91,113)=c[3]; M.at<double>(91,114)=c[2]; M.at<double>(91,115)=c[5]; M.at<double>(91,116)=c[17];
M.at<double>(92,17)=u[4]; M.at<double>(92,28)=a[6]; M.at<double>(92,29)=a[16]; M.at<double>(92,41)=a[17]; M.at<double>(92,42)=a[5]; M.at<double>(92,44)=a[14]; M.at<double>(92,45)=a[8]; M.at<double>(92,46)=a[4]; M.at<double>(92,47)=a[12]; M.at<double>(92,56)=b[5]; M.at<double>(92,57)=b[6]; M.at<double>(92,60)=b[12]; M.at<double>(92,62)=b[17]; M.at<double>(92,63)=b[8]; M.at<double>(92,66)=b[4]; M.at<double>(92,68)=b[14]; M.at<double>(92,74)=b[16]; M.at<double>(92,90)=c[12]; M.at<double>(92,94)=c[16]; M.at<double>(92,97)=c[14]; M.at<double>(92,101)=c[3]; M.at<double>(92,102)=c[4]; M.at<double>(92,106)=c[5]; M.at<double>(92,108)=c[15]; M.at<double>(92,109)=c[18]; M.at<double>(92,111)=c[17]; M.at<double>(92,112)=c[8]; M.at<double>(92,114)=c[6];
M.at<double>(93,17)=u[3]; M.at<double>(93,28)=a[7]; M.at<double>(93,29)=a[12]; M.at<double>(93,41)=a[18]; M.at<double>(93,42)=a[15]; M.at<double>(93,44)=a[5]; M.at<double>(93,45)=a[17]; M.at<double>(93,46)=a[3]; M.at<double>(93,47)=a[6]; M.at<double>(93,56)=b[15]; M.at<double>(93,57)=b[7]; M.at<double>(93,60)=b[6]; M.at<double>(93,62)=b[18]; M.at<double>(93,63)=b[17]; M.at<double>(93,66)=b[3]; M.at<double>(93,67)=b[4]; M.at<double>(93,68)=b[5]; M.at<double>(93,69)=b[8]; M.at<double>(93,70)=b[14]; M.at<double>(93,74)=b[12]; M.at<double>(93,90)=c[6]; M.at<double>(93,91)=c[14]; M.at<double>(93,94)=c[12]; M.at<double>(93,97)=c[5]; M.at<double>(93,102)=c[3]; M.at<double>(93,104)=c[4]; M.at<double>(93,106)=c[15]; M.at<double>(93,110)=c[8]; M.at<double>(93,111)=c[18]; M.at<double>(93,112)=c[17]; M.at<double>(93,114)=c[7];
M.at<double>(94,31)=a[2]; M.at<double>(94,37)=a[19]; M.at<double>(94,40)=a[20]; M.at<double>(94,48)=a[17]; M.at<double>(94,58)=b[2]; M.at<double>(94,72)=b[20]; M.at<double>(94,77)=b[19]; M.at<double>(94,81)=b[17]; M.at<double>(94,85)=c[17]; M.at<double>(94,95)=c[20]; M.at<double>(94,98)=c[19]; M.at<double>(94,115)=c[2];
M.at<double>(95,26)=u[4]; M.at<double>(95,27)=a[6]; M.at<double>(95,40)=a[12]; M.at<double>(95,42)=a[17]; M.at<double>(95,44)=a[8]; M.at<double>(95,46)=a[19]; M.at<double>(95,52)=a[16]; M.at<double>(95,55)=b[6]; M.at<double>(95,56)=b[17]; M.at<double>(95,66)=b[19]; M.at<double>(95,68)=b[8]; M.at<double>(95,72)=b[12]; M.at<double>(95,73)=b[16]; M.at<double>(95,93)=c[16]; M.at<double>(95,95)=c[12]; M.at<double>(95,97)=c[8]; M.at<double>(95,101)=c[2]; M.at<double>(95,102)=c[19]; M.at<double>(95,106)=c[17]; M.at<double>(95,108)=c[18]; M.at<double>(95,113)=c[6];
M.at<double>(96,23)=u[4]; M.at<double>(96,27)=a[5]; M.at<double>(96,28)=a[17]; M.at<double>(96,32)=a[12]; M.at<double>(96,38)=a[16]; M.at<double>(96,40)=a[14]; M.at<double>(96,41)=a[19]; M.at<double>(96,42)=a[4]; M.at<double>(96,47)=a[8]; M.at<double>(96,55)=b[5]; M.at<double>(96,56)=b[4]; M.at<double>(96,57)=b[17]; M.at<double>(96,60)=b[8]; M.at<double>(96,61)=b[12]; M.at<double>(96,62)=b[19]; M.at<double>(96,72)=b[14]; M.at<double>(96,78)=b[16]; M.at<double>(96,90)=c[8]; M.at<double>(96,92)=c[16]; M.at<double>(96,95)=c[14]; M.at<double>(96,101)=c[13]; M.at<double>(96,106)=c[4]; M.at<double>(96,108)=c[3]; M.at<double>(96,109)=c[2]; M.at<double>(96,111)=c[19]; M.at<double>(96,113)=c[5]; M.at<double>(96,114)=c[17]; M.at<double>(96,116)=c[12];
M.at<double>(97,42)=a[6]; M.at<double>(97,44)=a[12]; M.at<double>(97,46)=a[17]; M.at<double>(97,56)=b[6]; M.at<double>(97,66)=b[17]; M.at<double>(97,67)=b[8]; M.at<double>(97,68)=b[12]; M.at<double>(97,70)=b[16]; M.at<double>(97,91)=c[16]; M.at<double>(97,97)=c[12]; M.at<double>(97,101)=c[18]; M.at<double>(97,102)=c[17]; M.at<double>(97,104)=c[8]; M.at<double>(97,106)=c[6]; M.at<double>(97,108)=c[7];
M.at<double>(98,28)=a[12]; M.at<double>(98,41)=a[8]; M.at<double>(98,42)=a[14]; M.at<double>(98,47)=a[16]; M.at<double>(98,56)=b[14]; M.at<double>(98,57)=b[12]; M.at<double>(98,60)=b[16]; M.at<double>(98,62)=b[8]; M.at<double>(98,90)=c[16]; M.at<double>(98,101)=c[4]; M.at<double>(98,105)=c[3]; M.at<double>(98,106)=c[14]; M.at<double>(98,108)=c[5]; M.at<double>(98,109)=c[17]; M.at<double>(98,111)=c[8]; M.at<double>(98,114)=c[12];
M.at<double>(99,42)=a[7]; M.at<double>(99,44)=a[6]; M.at<double>(99,46)=a[18]; M.at<double>(99,56)=b[7]; M.at<double>(99,64)=b[8]; M.at<double>(99,66)=b[18]; M.at<double>(99,67)=b[17]; M.at<double>(99,68)=b[6]; M.at<double>(99,70)=b[12]; M.at<double>(99,91)=c[12]; M.at<double>(99,97)=c[6]; M.at<double>(99,102)=c[18]; M.at<double>(99,103)=c[8]; M.at<double>(99,104)=c[17]; M.at<double>(99,106)=c[7];
M.at<double>(100,51)=a[16]; M.at<double>(100,65)=b[16]; M.at<double>(100,89)=c[16]; M.at<double>(100,105)=c[10]; M.at<double>(100,109)=c[14];
M.at<double>(101,37)=a[9]; M.at<double>(101,38)=a[13]; M.at<double>(101,39)=a[20]; M.at<double>(101,48)=a[11]; M.at<double>(101,50)=a[3]; M.at<double>(101,53)=a[2]; M.at<double>(101,77)=b[9]; M.at<double>(101,78)=b[13]; M.at<double>(101,79)=b[20]; M.at<double>(101,80)=b[3]; M.at<double>(101,81)=b[11]; M.at<double>(101,82)=b[2]; M.at<double>(101,85)=c[11]; M.at<double>(101,86)=c[3]; M.at<double>(101,87)=c[2]; M.at<double>(101,92)=c[13]; M.at<double>(101,96)=c[20]; M.at<double>(101,98)=c[9];
M.at<double>(102,37)=a[13]; M.at<double>(102,38)=a[20]; M.at<double>(102,48)=a[3]; M.at<double>(102,50)=a[2]; M.at<double>(102,77)=b[13]; M.at<double>(102,78)=b[20]; M.at<double>(102,80)=b[2]; M.at<double>(102,81)=b[3]; M.at<double>(102,85)=c[3]; M.at<double>(102,86)=c[2]; M.at<double>(102,92)=c[20]; M.at<double>(102,98)=c[13];
M.at<double>(103,37)=a[20]; M.at<double>(103,48)=a[2]; M.at<double>(103,77)=b[20]; M.at<double>(103,81)=b[2]; M.at<double>(103,85)=c[2]; M.at<double>(103,98)=c[20];
M.at<double>(104,11)=u[4]; M.at<double>(104,28)=a[10]; M.at<double>(104,33)=a[14]; M.at<double>(104,34)=a[8]; M.at<double>(104,36)=a[16]; M.at<double>(104,51)=a[4]; M.at<double>(104,54)=b[8]; M.at<double>(104,57)=b[10]; M.at<double>(104,59)=b[14]; M.at<double>(104,65)=b[4]; M.at<double>(104,84)=b[16]; M.at<double>(104,88)=c[16]; M.at<double>(104,89)=c[4]; M.at<double>(104,99)=c[8]; M.at<double>(104,108)=c[1]; M.at<double>(104,109)=c[9]; M.at<double>(104,114)=c[10]; M.at<double>(104,117)=c[14];
M.at<double>(105,20)=u[4]; M.at<double>(105,27)=a[10]; M.at<double>(105,28)=a[4]; M.at<double>(105,32)=a[14]; M.at<double>(105,33)=a[8]; M.at<double>(105,51)=a[19]; M.at<double>(105,53)=a[16]; M.at<double>(105,55)=b[10]; M.at<double>(105,57)=b[4]; M.at<double>(105,59)=b[8]; M.at<double>(105,61)=b[14]; M.at<double>(105,65)=b[19]; M.at<double>(105,82)=b[16]; M.at<double>(105,87)=c[16]; M.at<double>(105,89)=c[19]; M.at<double>(105,108)=c[9]; M.at<double>(105,109)=c[13]; M.at<double>(105,113)=c[10]; M.at<double>(105,114)=c[4]; M.at<double>(105,116)=c[14]; M.at<double>(105,117)=c[8];
M.at<double>(106,27)=a[4]; M.at<double>(106,28)=a[19]; M.at<double>(106,31)=a[14]; M.at<double>(106,32)=a[8]; M.at<double>(106,50)=a[16]; M.at<double>(106,55)=b[4]; M.at<double>(106,57)=b[19]; M.at<double>(106,58)=b[14]; M.at<double>(106,61)=b[8]; M.at<double>(106,80)=b[16]; M.at<double>(106,86)=c[16]; M.at<double>(106,108)=c[13]; M.at<double>(106,109)=c[20]; M.at<double>(106,113)=c[4]; M.at<double>(106,114)=c[19]; M.at<double>(106,115)=c[14]; M.at<double>(106,116)=c[8];
M.at<double>(107,27)=a[19]; M.at<double>(107,31)=a[8]; M.at<double>(107,48)=a[16]; M.at<double>(107,55)=b[19]; M.at<double>(107,58)=b[8]; M.at<double>(107,81)=b[16]; M.at<double>(107,85)=c[16]; M.at<double>(107,108)=c[20]; M.at<double>(107,113)=c[19]; M.at<double>(107,115)=c[8];
M.at<double>(108,36)=a[20]; M.at<double>(108,48)=a[1]; M.at<double>(108,50)=a[9]; M.at<double>(108,53)=a[13]; M.at<double>(108,80)=b[9]; M.at<double>(108,81)=b[1]; M.at<double>(108,82)=b[13]; M.at<double>(108,84)=b[20]; M.at<double>(108,85)=c[1]; M.at<double>(108,86)=c[9]; M.at<double>(108,87)=c[13]; M.at<double>(108,88)=c[20];
M.at<double>(109,26)=u[3]; M.at<double>(109,27)=a[7]; M.at<double>(109,40)=a[6]; M.at<double>(109,42)=a[18]; M.at<double>(109,44)=a[17]; M.at<double>(109,46)=a[2]; M.at<double>(109,52)=a[12]; M.at<double>(109,55)=b[7]; M.at<double>(109,56)=b[18]; M.at<double>(109,66)=b[2]; M.at<double>(109,67)=b[19]; M.at<double>(109,68)=b[17]; M.at<double>(109,70)=b[8]; M.at<double>(109,72)=b[6]; M.at<double>(109,73)=b[12]; M.at<double>(109,91)=c[8]; M.at<double>(109,93)=c[12]; M.at<double>(109,95)=c[6]; M.at<double>(109,97)=c[17]; M.at<double>(109,102)=c[2]; M.at<double>(109,104)=c[19]; M.at<double>(109,106)=c[18]; M.at<double>(109,113)=c[7];
M.at<double>(110,7)=u[3]; M.at<double>(110,30)=a[5]; M.at<double>(110,34)=a[7]; M.at<double>(110,43)=a[15]; M.at<double>(110,45)=a[11]; M.at<double>(110,49)=a[6]; M.at<double>(110,54)=b[7]; M.at<double>(110,63)=b[11]; M.at<double>(110,67)=b[1]; M.at<double>(110,69)=b[10]; M.at<double>(110,71)=b[15]; M.at<double>(110,75)=b[5]; M.at<double>(110,76)=b[6]; M.at<double>(110,99)=c[7]; M.at<double>(110,100)=c[15]; M.at<double>(110,104)=c[1]; M.at<double>(110,107)=c[5]; M.at<double>(110,110)=c[10]; M.at<double>(110,112)=c[11]; M.at<double>(110,119)=c[6];
M.at<double>(111,18)=u[2]; M.at<double>(111,35)=a[20]; M.at<double>(111,36)=a[13]; M.at<double>(111,50)=a[1]; M.at<double>(111,53)=a[9]; M.at<double>(111,80)=b[1]; M.at<double>(111,82)=b[9]; M.at<double>(111,83)=b[20]; M.at<double>(111,84)=b[13]; M.at<double>(111,86)=c[1]; M.at<double>(111,87)=c[9]; M.at<double>(111,88)=c[13]; M.at<double>(111,118)=c[20];
M.at<double>(112,19)=u[2]; M.at<double>(112,31)=a[1]; M.at<double>(112,32)=a[9]; M.at<double>(112,33)=a[13]; M.at<double>(112,34)=a[20]; M.at<double>(112,36)=a[19]; M.at<double>(112,50)=a[10]; M.at<double>(112,53)=a[4]; M.at<double>(112,54)=b[20]; M.at<double>(112,58)=b[1]; M.at<double>(112,59)=b[13]; M.at<double>(112,61)=b[9]; M.at<double>(112,80)=b[10]; M.at<double>(112,82)=b[4]; M.at<double>(112,84)=b[19]; M.at<double>(112,86)=c[10]; M.at<double>(112,87)=c[4]; M.at<double>(112,88)=c[19]; M.at<double>(112,99)=c[20]; M.at<double>(112,115)=c[1]; M.at<double>(112,116)=c[9]; M.at<double>(112,117)=c[13];
M.at<double>(113,31)=a[9]; M.at<double>(113,32)=a[13]; M.at<double>(113,33)=a[20]; M.at<double>(113,48)=a[10]; M.at<double>(113,50)=a[4]; M.at<double>(113,53)=a[19]; M.at<double>(113,58)=b[9]; M.at<double>(113,59)=b[20]; M.at<double>(113,61)=b[13]; M.at<double>(113,80)=b[4]; M.at<double>(113,81)=b[10]; M.at<double>(113,82)=b[19]; M.at<double>(113,85)=c[10]; M.at<double>(113,86)=c[4]; M.at<double>(113,87)=c[19]; M.at<double>(113,115)=c[9]; M.at<double>(113,116)=c[13]; M.at<double>(113,117)=c[20];
M.at<double>(114,31)=a[13]; M.at<double>(114,32)=a[20]; M.at<double>(114,48)=a[4]; M.at<double>(114,50)=a[19]; M.at<double>(114,58)=b[13]; M.at<double>(114,61)=b[20]; M.at<double>(114,80)=b[19]; M.at<double>(114,81)=b[4]; M.at<double>(114,85)=c[4]; M.at<double>(114,86)=c[19]; M.at<double>(114,115)=c[13]; M.at<double>(114,116)=c[20];
M.at<double>(115,31)=a[20]; M.at<double>(115,48)=a[19]; M.at<double>(115,58)=b[20]; M.at<double>(115,81)=b[19]; M.at<double>(115,85)=c[19]; M.at<double>(115,115)=c[20];
M.at<double>(116,24)=u[2]; M.at<double>(116,29)=a[13]; M.at<double>(116,30)=a[20]; M.at<double>(116,37)=a[11]; M.at<double>(116,38)=a[3]; M.at<double>(116,39)=a[2]; M.at<double>(116,50)=a[15]; M.at<double>(116,52)=a[9]; M.at<double>(116,53)=a[18]; M.at<double>(116,73)=b[9]; M.at<double>(116,74)=b[13]; M.at<double>(116,75)=b[20]; M.at<double>(116,77)=b[11]; M.at<double>(116,78)=b[3]; M.at<double>(116,79)=b[2]; M.at<double>(116,80)=b[15]; M.at<double>(116,82)=b[18]; M.at<double>(116,86)=c[15]; M.at<double>(116,87)=c[18]; M.at<double>(116,92)=c[3]; M.at<double>(116,93)=c[9]; M.at<double>(116,94)=c[13]; M.at<double>(116,96)=c[2]; M.at<double>(116,98)=c[11]; M.at<double>(116,107)=c[20];
M.at<double>(117,29)=a[20]; M.at<double>(117,37)=a[3]; M.at<double>(117,38)=a[2]; M.at<double>(117,48)=a[15]; M.at<double>(117,50)=a[18]; M.at<double>(117,52)=a[13]; M.at<double>(117,73)=b[13]; M.at<double>(117,74)=b[20]; M.at<double>(117,77)=b[3]; M.at<double>(117,78)=b[2]; M.at<double>(117,80)=b[18]; M.at<double>(117,81)=b[15]; M.at<double>(117,85)=c[15]; M.at<double>(117,86)=c[18]; M.at<double>(117,92)=c[2]; M.at<double>(117,93)=c[13]; M.at<double>(117,94)=c[20]; M.at<double>(117,98)=c[3];
M.at<double>(118,27)=a[13]; M.at<double>(118,28)=a[20]; M.at<double>(118,31)=a[4]; M.at<double>(118,32)=a[19]; M.at<double>(118,48)=a[14]; M.at<double>(118,50)=a[8]; M.at<double>(118,55)=b[13]; M.at<double>(118,57)=b[20]; M.at<double>(118,58)=b[4]; M.at<double>(118,61)=b[19]; M.at<double>(118,80)=b[8]; M.at<double>(118,81)=b[14]; M.at<double>(118,85)=c[14]; M.at<double>(118,86)=c[8]; M.at<double>(118,113)=c[13]; M.at<double>(118,114)=c[20]; M.at<double>(118,115)=c[4]; M.at<double>(118,116)=c[19];
M.at<double>(119,27)=a[20]; M.at<double>(119,31)=a[19]; M.at<double>(119,48)=a[8]; M.at<double>(119,55)=b[20]; M.at<double>(119,58)=b[19]; M.at<double>(119,81)=b[8]; M.at<double>(119,85)=c[8]; M.at<double>(119,113)=c[20]; M.at<double>(119,115)=c[19];
return M.t();
}
cv::Mat dls::Hessian(const double s[])
{
// the vector of monomials is
// m = [ const ; s1^2 * s2 ; s1 * s2 ; s1 * s3 ; s2 * s3 ; s2^2 * s3 ; s2^3 ; ...
// s1 * s3^2 ; s1 ; s3 ; s2 ; s2 * s3^2 ; s1^2 ; s3^2 ; s2^2 ; s3^3 ; ...
// s1 * s2 * s3 ; s1 * s2^2 ; s1^2 * s3 ; s1^3]
//
// deriv of m w.r.t. s1
//Hs3 = [0 ; 2 * s(1) * s(2) ; s(2) ; s(3) ; 0 ; 0 ; 0 ; ...
// s(3)^2 ; 1 ; 0 ; 0 ; 0 ; 2 * s(1) ; 0 ; 0 ; 0 ; ...
// s(2) * s(3) ; s(2)^2 ; 2*s(1)*s(3); 3 * s(1)^2];
double Hs1[20];
Hs1[0]=0; Hs1[1]=2*s[0]*s[1]; Hs1[2]=s[1]; Hs1[3]=s[2]; Hs1[4]=0; Hs1[5]=0; Hs1[6]=0;
Hs1[7]=s[2]*s[2]; Hs1[8]=1; Hs1[9]=0; Hs1[10]=0; Hs1[11]=0; Hs1[12]=2*s[0]; Hs1[13]=0;
Hs1[14]=0; Hs1[15]=0; Hs1[16]=s[1]*s[2]; Hs1[17]=s[1]*s[1]; Hs1[18]=2*s[0]*s[2]; Hs1[19]=3*s[0]*s[0];
// deriv of m w.r.t. s2
//Hs2 = [0 ; s(1)^2 ; s(1) ; 0 ; s(3) ; 2 * s(2) * s(3) ; 3 * s(2)^2 ; ...
// 0 ; 0 ; 0 ; 1 ; s(3)^2 ; 0 ; 0 ; 2 * s(2) ; 0 ; ...
// s(1) * s(3) ; s(1) * 2 * s(2) ; 0 ; 0];
double Hs2[20];
Hs2[0]=0; Hs2[1]=s[0]*s[0]; Hs2[2]=s[0]; Hs2[3]=0; Hs2[4]=s[2]; Hs2[5]=2*s[1]*s[2]; Hs2[6]=3*s[1]*s[1];
Hs2[7]=0; Hs2[8]=0; Hs2[9]=0; Hs2[10]=1; Hs2[11]=s[2]*s[2]; Hs2[12]=0; Hs2[13]=0;
Hs2[14]=2*s[1]; Hs2[15]=0; Hs2[16]=s[0]*s[2]; Hs2[17]=2*s[0]*s[1]; Hs2[18]=0; Hs2[19]=0;
// deriv of m w.r.t. s3
//Hs3 = [0 ; 0 ; 0 ; s(1) ; s(2) ; s(2)^2 ; 0 ; ...
// s(1) * 2 * s(3) ; 0 ; 1 ; 0 ; s(2) * 2 * s(3) ; 0 ; 2 * s(3) ; 0 ; 3 * s(3)^2 ; ...
// s(1) * s(2) ; 0 ; s(1)^2 ; 0];
double Hs3[20];
Hs3[0]=0; Hs3[1]=0; Hs3[2]=0; Hs3[3]=s[0]; Hs3[4]=s[1]; Hs3[5]=s[1]*s[1]; Hs3[6]=0;
Hs3[7]=2*s[0]*s[2]; Hs3[8]=0; Hs3[9]=1; Hs3[10]=0; Hs3[11]=2*s[1]*s[2]; Hs3[12]=0; Hs3[13]=2*s[2];
Hs3[14]=0; Hs3[15]=3*s[2]*s[2]; Hs3[16]=s[0]*s[1]; Hs3[17]=0; Hs3[18]=s[0]*s[0]; Hs3[19]=0;
// fill Hessian matrix
cv::Mat H(3, 3, CV_64F);
H.at<double>(0,0) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
H.at<double>(0,1) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
H.at<double>(0,2) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
H.at<double>(1,0) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
H.at<double>(1,1) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
H.at<double>(1,2) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
H.at<double>(2,0) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at<double>(0,0);
H.at<double>(2,1) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at<double>(0,0);
H.at<double>(2,2) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at<double>(0,0);
return H;
}
cv::Mat dls::cayley2rotbar(const cv::Mat& s)
{
double s_mul1 = cv::Mat(s.t()*s).at<double>(0,0);
cv::Mat s_mul2 = s*s.t();
cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
return cv::Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t();
}
cv::Mat dls::skewsymm(const cv::Mat * X1)
{
cv::MatConstIterator_<double> it = X1->begin<double>();
return (cv::Mat_<double>(3,3) << 0, -*(it+2), *(it+1),
*(it+2), 0, -*(it+0),
-*(it+1), *(it+0), 0);
}
cv::Mat dls::rotx(const double t)
{
// rotx: rotation about y-axis
double ct = cos(t);
double st = sin(t);
return (cv::Mat_<double>(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct);
}
cv::Mat dls::roty(const double t)
{
// roty: rotation about y-axis
double ct = cos(t);
double st = sin(t);
return (cv::Mat_<double>(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct);
}
cv::Mat dls::rotz(const double t)
{
// rotz: rotation about y-axis
double ct = cos(t);
double st = sin(t);
return (cv::Mat_<double>(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1);
}
cv::Mat dls::mean(const cv::Mat& M)
{
cv::Mat m = cv::Mat::zeros(3, 1, CV_64F);
for (int i = 0; i < M.cols; ++i) m += M.col(i);
return m.mul(1./(double)M.cols);
}
bool dls::is_empty(const cv::Mat * M)
{
cv::MatConstIterator_<double> it = M->begin<double>(), it_end = M->end<double>();
for(; it != it_end; ++it)
{
if(*it < 0) return false;
}
return true;
}
bool dls::positive_eigenvalues(const cv::Mat * eigenvalues)
{
cv::MatConstIterator_<double> it = eigenvalues->begin<double>();
return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0;
}

773
Lib/opencv/sources/modules/calib3d/src/dls.h Normal file
View File

@@ -0,0 +1,773 @@
#ifndef DLS_H_
#define DLS_H_
#include "precomp.hpp"
#include <iostream>
using namespace std;
using namespace cv;
class dls
{
public:
dls(const cv::Mat& opoints, const cv::Mat& ipoints);
~dls();
bool compute_pose(cv::Mat& R, cv::Mat& t);
private:
// initialisation
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
for(int i = 0; i < N; i++)
{
p.at<double>(0,i) = opoints.at<OpointType>(i).x;
p.at<double>(1,i) = opoints.at<OpointType>(i).y;
p.at<double>(2,i) = opoints.at<OpointType>(i).z;
// compute mean of object points
mn.at<double>(0) += p.at<double>(0,i);
mn.at<double>(1) += p.at<double>(1,i);
mn.at<double>(2) += p.at<double>(2,i);
// make z into unit vectors from normalized pixel coords
double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
sr = std::sqrt(sr);
z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
z.at<double>(2,i) = (double)1 / sr;
}
mn.at<double>(0) /= (double)N;
mn.at<double>(1) /= (double)N;
mn.at<double>(2) /= (double)N;
}
// main algorithm
cv::Mat LeftMultVec(const cv::Mat& v);
void run_kernel(const cv::Mat& pp);
void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
void fill_coeff(const cv::Mat * D);
// useful functions
cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
const std::vector<double>& c, const std::vector<double>& u);
cv::Mat Hessian(const double s[]);
cv::Mat cayley2rotbar(const cv::Mat& s);
cv::Mat skewsymm(const cv::Mat * X1);
// extra functions
cv::Mat rotx(const double t);
cv::Mat roty(const double t);
cv::Mat rotz(const double t);
cv::Mat mean(const cv::Mat& M);
bool is_empty(const cv::Mat * v);
bool positive_eigenvalues(const cv::Mat * eigenvalues);
cv::Mat p, z, mn; // object-image points
int N; // number of input points
std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
std::vector<cv::Mat> C_est_, t_est_; // optimal candidates
cv::Mat C_est__, t_est__; // optimal found solution
double cost__; // optimal found solution
};
class EigenvalueDecomposition {
private:
// Holds the data dimension.
int n;
// Stores real/imag part of a complex division.
double cdivr, cdivi;
// Pointer to internal memory.
double *d, *e, *ort;
double **V, **H;
// Holds the computed eigenvalues.
Mat _eigenvalues;
// Holds the computed eigenvectors.
Mat _eigenvectors;
// Allocates memory.
template<typename _Tp>
_Tp *alloc_1d(int m) {
return new _Tp[m];
}
// Allocates memory.
template<typename _Tp>
_Tp *alloc_1d(int m, _Tp val) {
_Tp *arr = alloc_1d<_Tp> (m);
for (int i = 0; i < m; i++)
arr[i] = val;
return arr;
}
// Allocates memory.
template<typename _Tp>
_Tp **alloc_2d(int m, int _n) {
_Tp **arr = new _Tp*[m];
for (int i = 0; i < m; i++)
arr[i] = new _Tp[_n];
return arr;
}
// Allocates memory.
template<typename _Tp>
_Tp **alloc_2d(int m, int _n, _Tp val) {
_Tp **arr = alloc_2d<_Tp> (m, _n);
for (int i = 0; i < m; i++) {
for (int j = 0; j < _n; j++) {
arr[i][j] = val;
}
}
return arr;
}
void cdiv(double xr, double xi, double yr, double yi) {
double r, dv;
if (std::abs(yr) > std::abs(yi)) {
r = yi / yr;
dv = yr + r * yi;
cdivr = (xr + r * xi) / dv;
cdivi = (xi - r * xr) / dv;
} else {
r = yr / yi;
dv = yi + r * yr;
cdivr = (r * xr + xi) / dv;
cdivi = (r * xi - xr) / dv;
}
}
// Nonsymmetric reduction from Hessenberg to real Schur form.
void hqr2() {
// This is derived from the Algol procedure hqr2,
// by Martin and Wilkinson, Handbook for Auto. Comp.,
// Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
// Initialize
int nn = this->n;
int n1 = nn - 1;
int low = 0;
int high = nn - 1;
double eps = std::pow(2.0, -52.0);
double exshift = 0.0;
double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;
// Store roots isolated by balanc and compute matrix norm
double norm = 0.0;
for (int i = 0; i < nn; i++) {
if (i < low || i > high) {
d[i] = H[i][i];
e[i] = 0.0;
}
for (int j = std::max(i - 1, 0); j < nn; j++) {
norm = norm + std::abs(H[i][j]);
}
}
// Outer loop over eigenvalue index
int iter = 0;
while (n1 >= low) {
// Look for single small sub-diagonal element
int l = n1;
while (l > low) {
s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
if (s == 0.0) {
s = norm;
}
if (std::abs(H[l][l - 1]) < eps * s) {
break;
}
l--;
}
// Check for convergence
// One root found
if (l == n1) {
H[n1][n1] = H[n1][n1] + exshift;
d[n1] = H[n1][n1];
e[n1] = 0.0;
n1--;
iter = 0;
// Two roots found
} else if (l == n1 - 1) {
w = H[n1][n1 - 1] * H[n1 - 1][n1];
p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0;
q = p * p + w;
z = std::sqrt(std::abs(q));
H[n1][n1] = H[n1][n1] + exshift;
H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift;
x = H[n1][n1];
// Real pair
if (q >= 0) {
if (p >= 0) {
z = p + z;
} else {
z = p - z;
}
d[n1 - 1] = x + z;
d[n1] = d[n1 - 1];
if (z != 0.0) {
d[n1] = x - w / z;
}
e[n1 - 1] = 0.0;
e[n1] = 0.0;
x = H[n1][n1 - 1];
s = std::abs(x) + std::abs(z);
p = x / s;
q = z / s;
r = std::sqrt(p * p + q * q);
p = p / r;
q = q / r;
// Row modification
for (int j = n1 - 1; j < nn; j++) {
z = H[n1 - 1][j];
H[n1 - 1][j] = q * z + p * H[n1][j];
H[n1][j] = q * H[n1][j] - p * z;
}
// Column modification
for (int i = 0; i <= n1; i++) {
z = H[i][n1 - 1];
H[i][n1 - 1] = q * z + p * H[i][n1];
H[i][n1] = q * H[i][n1] - p * z;
}
// Accumulate transformations
for (int i = low; i <= high; i++) {
z = V[i][n1 - 1];
V[i][n1 - 1] = q * z + p * V[i][n1];
V[i][n1] = q * V[i][n1] - p * z;
}
// Complex pair
} else {
d[n1 - 1] = x + p;
d[n1] = x + p;
e[n1 - 1] = z;
e[n1] = -z;
}
n1 = n1 - 2;
iter = 0;
// No convergence yet
} else {
// Form shift
x = H[n1][n1];
y = 0.0;
w = 0.0;
if (l < n1) {
y = H[n1 - 1][n1 - 1];
w = H[n1][n1 - 1] * H[n1 - 1][n1];
}
// Wilkinson's original ad hoc shift
if (iter == 10) {
exshift += x;
for (int i = low; i <= n1; i++) {
H[i][i] -= x;
}
s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]);
x = y = 0.75 * s;
w = -0.4375 * s * s;
}
// MATLAB's new ad hoc shift
if (iter == 30) {
s = (y - x) / 2.0;
s = s * s + w;
if (s > 0) {
s = std::sqrt(s);
if (y < x) {
s = -s;
}
s = x - w / ((y - x) / 2.0 + s);
for (int i = low; i <= n1; i++) {
H[i][i] -= s;
}
exshift += s;
x = y = w = 0.964;
}
}
iter = iter + 1; // (Could check iteration count here.)
// Look for two consecutive small sub-diagonal elements
int m = n1 - 2;
while (m >= l) {
z = H[m][m];
r = x - z;
s = y - z;
p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
q = H[m + 1][m + 1] - z - r - s;
r = H[m + 2][m + 1];
s = std::abs(p) + std::abs(q) + std::abs(r);
p = p / s;
q = q / s;
r = r / s;
if (m == l) {
break;
}
if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p)
* (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(
H[m + 1][m + 1])))) {
break;
}
m--;
}
for (int i = m + 2; i <= n1; i++) {
H[i][i - 2] = 0.0;
if (i > m + 2) {
H[i][i - 3] = 0.0;
}
}
// Double QR step involving rows l:n and columns m:n
for (int k = m; k <= n1 - 1; k++) {
bool notlast = (k != n1 - 1);
if (k != m) {
p = H[k][k - 1];
q = H[k + 1][k - 1];
r = (notlast ? H[k + 2][k - 1] : 0.0);
x = std::abs(p) + std::abs(q) + std::abs(r);
if (x != 0.0) {
p = p / x;
q = q / x;
r = r / x;
}
}
if (x == 0.0) {
break;
}
s = std::sqrt(p * p + q * q + r * r);
if (p < 0) {
s = -s;
}
if (s != 0) {
if (k != m) {
H[k][k - 1] = -s * x;
} else if (l != m) {
H[k][k - 1] = -H[k][k - 1];
}
p = p + s;
x = p / s;
y = q / s;
z = r / s;
q = q / p;
r = r / p;
// Row modification
for (int j = k; j < nn; j++) {
p = H[k][j] + q * H[k + 1][j];
if (notlast) {
p = p + r * H[k + 2][j];
H[k + 2][j] = H[k + 2][j] - p * z;
}
H[k][j] = H[k][j] - p * x;
H[k + 1][j] = H[k + 1][j] - p * y;
}
// Column modification
for (int i = 0; i <= std::min(n1, k + 3); i++) {
p = x * H[i][k] + y * H[i][k + 1];
if (notlast) {
p = p + z * H[i][k + 2];
H[i][k + 2] = H[i][k + 2] - p * r;
}
H[i][k] = H[i][k] - p;
H[i][k + 1] = H[i][k + 1] - p * q;
}
// Accumulate transformations
for (int i = low; i <= high; i++) {
p = x * V[i][k] + y * V[i][k + 1];
if (notlast) {
p = p + z * V[i][k + 2];
V[i][k + 2] = V[i][k + 2] - p * r;
}
V[i][k] = V[i][k] - p;
V[i][k + 1] = V[i][k + 1] - p * q;
}
} // (s != 0)
} // k loop
} // check convergence
} // while (n1 >= low)
// Backsubstitute to find vectors of upper triangular form
if (norm == 0.0) {
return;
}
for (n1 = nn - 1; n1 >= 0; n1--) {
p = d[n1];
q = e[n1];
// Real vector
if (q == 0) {
int l = n1;
H[n1][n1] = 1.0;
for (int i = n1 - 1; i >= 0; i--) {
w = H[i][i] - p;
r = 0.0;
for (int j = l; j <= n1; j++) {
r = r + H[i][j] * H[j][n1];
}
if (e[i] < 0.0) {
z = w;
s = r;
} else {
l = i;
if (e[i] == 0.0) {
if (w != 0.0) {
H[i][n1] = -r / w;
} else {
H[i][n1] = -r / (eps * norm);
}
// Solve real equations
} else {
x = H[i][i + 1];
y = H[i + 1][i];
q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
t = (x * s - z * r) / q;
H[i][n1] = t;
if (std::abs(x) > std::abs(z)) {
H[i + 1][n1] = (-r - w * t) / x;
} else {
H[i + 1][n1] = (-s - y * t) / z;
}
}
// Overflow control
t = std::abs(H[i][n1]);
if ((eps * t) * t > 1) {
for (int j = i; j <= n1; j++) {
H[j][n1] = H[j][n1] / t;
}
}
}
}
// Complex vector
} else if (q < 0) {
int l = n1 - 1;
// Last vector component imaginary so matrix is triangular
if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) {
H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1];
H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1];
} else {
cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q);
H[n1 - 1][n1 - 1] = cdivr;
H[n1 - 1][n1] = cdivi;
}
H[n1][n1 - 1] = 0.0;
H[n1][n1] = 1.0;
for (int i = n1 - 2; i >= 0; i--) {
double ra, sa;
ra = 0.0;
sa = 0.0;
for (int j = l; j <= n1; j++) {
ra = ra + H[i][j] * H[j][n1 - 1];
sa = sa + H[i][j] * H[j][n1];
}
w = H[i][i] - p;
if (e[i] < 0.0) {
z = w;
r = ra;
s = sa;
} else {
l = i;
if (e[i] == 0) {
cdiv(-ra, -sa, w, q);
H[i][n1 - 1] = cdivr;
H[i][n1] = cdivi;
} else {
// Solve complex equations
x = H[i][i + 1];
y = H[i + 1][i];
double vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
double vi = (d[i] - p) * 2.0 * q;
if (vr == 0.0 && vi == 0.0) {
vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x)
+ std::abs(y) + std::abs(z));
}
cdiv(x * r - z * ra + q * sa,
x * s - z * sa - q * ra, vr, vi);
H[i][n1 - 1] = cdivr;
H[i][n1] = cdivi;
if (std::abs(x) > (std::abs(z) + std::abs(q))) {
H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q
* H[i][n1]) / x;
H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1
- 1]) / x;
} else {
cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z,
q);
H[i + 1][n1 - 1] = cdivr;
H[i + 1][n1] = cdivi;
}
}
// Overflow control
t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1]));
if ((eps * t) * t > 1) {
for (int j = i; j <= n1; j++) {
H[j][n1 - 1] = H[j][n1 - 1] / t;
H[j][n1] = H[j][n1] / t;
}
}
}
}
}
}
// Vectors of isolated roots
for (int i = 0; i < nn; i++) {
if (i < low || i > high) {
for (int j = i; j < nn; j++) {
V[i][j] = H[i][j];
}
}
}
// Back transformation to get eigenvectors of original matrix
for (int j = nn - 1; j >= low; j--) {
for (int i = low; i <= high; i++) {
z = 0.0;
for (int k = low; k <= std::min(j, high); k++) {
z = z + V[i][k] * H[k][j];
}
V[i][j] = z;
}
}
}
// Nonsymmetric reduction to Hessenberg form.
void orthes() {
// This is derived from the Algol procedures orthes and ortran,
// by Martin and Wilkinson, Handbook for Auto. Comp.,
// Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutines in EISPACK.
int low = 0;
int high = n - 1;
for (int m = low + 1; m <= high - 1; m++) {
// Scale column.
double scale = 0.0;
for (int i = m; i <= high; i++) {
scale = scale + std::abs(H[i][m - 1]);
}
if (scale != 0.0) {
// Compute Householder transformation.
double h = 0.0;
for (int i = high; i >= m; i--) {
ort[i] = H[i][m - 1] / scale;
h += ort[i] * ort[i];
}
double g = std::sqrt(h);
if (ort[m] > 0) {
g = -g;
}
h = h - ort[m] * g;
ort[m] = ort[m] - g;
// Apply Householder similarity transformation
// H = (I-u*u'/h)*H*(I-u*u')/h)
for (int j = m; j < n; j++) {
double f = 0.0;
for (int i = high; i >= m; i--) {
f += ort[i] * H[i][j];
}
f = f / h;
for (int i = m; i <= high; i++) {
H[i][j] -= f * ort[i];
}
}
for (int i = 0; i <= high; i++) {
double f = 0.0;
for (int j = high; j >= m; j--) {
f += ort[j] * H[i][j];
}
f = f / h;
for (int j = m; j <= high; j++) {
H[i][j] -= f * ort[j];
}
}
ort[m] = scale * ort[m];
H[m][m - 1] = scale * g;
}
}
// Accumulate transformations (Algol's ortran).
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
V[i][j] = (i == j ? 1.0 : 0.0);
}
}
for (int m = high - 1; m >= low + 1; m--) {
if (H[m][m - 1] != 0.0) {
for (int i = m + 1; i <= high; i++) {
ort[i] = H[i][m - 1];
}
for (int j = m; j <= high; j++) {
double g = 0.0;
for (int i = m; i <= high; i++) {
g += ort[i] * V[i][j];
}
// Double division avoids possible underflow
g = (g / ort[m]) / H[m][m - 1];
for (int i = m; i <= high; i++) {
V[i][j] += g * ort[i];
}
}
}
}
}
// Releases all internal working memory.
void release() {
// releases the working data
delete[] d;
delete[] e;
delete[] ort;
for (int i = 0; i < n; i++) {
delete[] H[i];
delete[] V[i];
}
delete[] H;
delete[] V;
}
// Computes the Eigenvalue Decomposition for a matrix given in H.
void compute() {
// Allocate memory for the working data.
V = alloc_2d<double> (n, n, 0.0);
d = alloc_1d<double> (n);
e = alloc_1d<double> (n);
ort = alloc_1d<double> (n);
// Reduce to Hessenberg form.
orthes();
// Reduce Hessenberg to real Schur form.
hqr2();
// Copy eigenvalues to OpenCV Matrix.
_eigenvalues.create(1, n, CV_64FC1);
for (int i = 0; i < n; i++) {
_eigenvalues.at<double> (0, i) = d[i];
}
// Copy eigenvectors to OpenCV Matrix.
_eigenvectors.create(n, n, CV_64FC1);
for (int i = 0; i < n; i++)
for (int j = 0; j < n; j++)
_eigenvectors.at<double> (i, j) = V[i][j];
// Deallocate the memory by releasing all internal working data.
release();
}
public:
EigenvalueDecomposition()
: n(0), cdivr(0), cdivi(0), d(0), e(0), ort(0), V(0), H(0) {}
// Initializes & computes the Eigenvalue Decomposition for a general matrix
// given in src. This function is a port of the EigenvalueSolver in JAMA,
// which has been released to public domain by The MathWorks and the
// National Institute of Standards and Technology (NIST).
EigenvalueDecomposition(InputArray src) {
compute(src);
}
// This function computes the Eigenvalue Decomposition for a general matrix
// given in src. This function is a port of the EigenvalueSolver in JAMA,
// which has been released to public domain by The MathWorks and the
// National Institute of Standards and Technology (NIST).
void compute(InputArray src)
{
/*if(isSymmetric(src)) {
// Fall back to OpenCV for a symmetric matrix!
cv::eigen(src, _eigenvalues, _eigenvectors);
} else {*/
Mat tmp;
// Convert the given input matrix to double. Is there any way to
// prevent allocating the temporary memory? Only used for copying
// into working memory and deallocated after.
src.getMat().convertTo(tmp, CV_64FC1);
// Get dimension of the matrix.
this->n = tmp.cols;
// Allocate the matrix data to work on.
this->H = alloc_2d<double> (n, n);
// Now safely copy the data.
for (int i = 0; i < tmp.rows; i++) {
for (int j = 0; j < tmp.cols; j++) {
this->H[i][j] = tmp.at<double>(i, j);
}
}
// Deallocates the temporary matrix before computing.
tmp.release();
// Performs the eigenvalue decomposition of H.
compute();
// }
}
~EigenvalueDecomposition() {}
// Returns the eigenvalues of the Eigenvalue Decomposition.
Mat eigenvalues() { return _eigenvalues; }
// Returns the eigenvectors of the Eigenvalue Decomposition.
Mat eigenvectors() { return _eigenvectors; }
};
#endif // DLS_H

632
Lib/opencv/sources/modules/calib3d/src/epnp.cpp Normal file
View File

@@ -0,0 +1,632 @@
#include <iostream>
#include "precomp.hpp"
#include "epnp.h"
namespace cv
{
epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
pws.resize(3 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<Point3f,Point2f>(opoints, ipoints);
else
init_points<Point3d,Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<Point3f,Point2d>(opoints, ipoints);
else
init_points<Point3d,Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
max_nr = 0;
A1 = NULL;
A2 = NULL;
}
epnp::~epnp()
{
if (A1)
delete[] A1;
if (A2)
delete[] A2;
}
void epnp::choose_control_points(void)
{
// Take C0 as the reference points centroid:
cws[0][0] = cws[0][1] = cws[0][2] = 0;
for(int i = 0; i < number_of_correspondences; i++)
for(int j = 0; j < 3; j++)
cws[0][j] += pws[3 * i + j];
for(int j = 0; j < 3; j++)
cws[0][j] /= number_of_correspondences;
// Take C1, C2, and C3 from PCA on the reference points:
CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);
double pw0tpw0[3 * 3] = {}, dc[3] = {}, uct[3 * 3] = {};
CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
CvMat DC = cvMat(3, 1, CV_64F, dc);
CvMat UCt = cvMat(3, 3, CV_64F, uct);
for(int i = 0; i < number_of_correspondences; i++)
for(int j = 0; j < 3; j++)
PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];
cvMulTransposed(PW0, &PW0tPW0, 1);
cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
cvReleaseMat(&PW0);
for(int i = 1; i < 4; i++) {
double k = sqrt(dc[i - 1] / number_of_correspondences);
for(int j = 0; j < 3; j++)
cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
}
}
void epnp::compute_barycentric_coordinates(void)
{
double cc[3 * 3] = {}, cc_inv[3 * 3] = {};
CvMat CC = cvMat(3, 3, CV_64F, cc);
CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);
for(int i = 0; i < 3; i++)
for(int j = 1; j < 4; j++)
cc[3 * i + j - 1] = cws[j][i] - cws[0][i];
cvInvert(&CC, &CC_inv, CV_SVD);
double * ci = cc_inv;
for(int i = 0; i < number_of_correspondences; i++) {
double * pi = &pws[0] + 3 * i;
double * a = &alphas[0] + 4 * i;
for(int j = 0; j < 3; j++)
{
a[1 + j] =
ci[3 * j ] * (pi[0] - cws[0][0]) +
ci[3 * j + 1] * (pi[1] - cws[0][1]) +
ci[3 * j + 2] * (pi[2] - cws[0][2]);
}
a[0] = 1.0f - a[1] - a[2] - a[3];
}
}
void epnp::fill_M(CvMat * M,
const int row, const double * as, const double u, const double v)
{
double * M1 = M->data.db + row * 12;
double * M2 = M1 + 12;
for(int i = 0; i < 4; i++) {
M1[3 * i ] = as[i] * fu;
M1[3 * i + 1] = 0.0;
M1[3 * i + 2] = as[i] * (uc - u);
M2[3 * i ] = 0.0;
M2[3 * i + 1] = as[i] * fv;
M2[3 * i + 2] = as[i] * (vc - v);
}
}
void epnp::compute_ccs(const double * betas, const double * ut)
{
for(int i = 0; i < 4; i++)
ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;
for(int i = 0; i < 4; i++) {
const double * v = ut + 12 * (11 - i);
for(int j = 0; j < 4; j++)
for(int k = 0; k < 3; k++)
ccs[j][k] += betas[i] * v[3 * j + k];
}
}
void epnp::compute_pcs(void)
{
for(int i = 0; i < number_of_correspondences; i++) {
double * a = &alphas[0] + 4 * i;
double * pc = &pcs[0] + 3 * i;
for(int j = 0; j < 3; j++)
pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];
}
}
void epnp::compute_pose(Mat& R, Mat& t)
{
choose_control_points();
compute_barycentric_coordinates();
CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
for(int i = 0; i < number_of_correspondences; i++)
fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);
double mtm[12 * 12] = {}, d[12] = {}, ut[12 * 12] = {};
CvMat MtM = cvMat(12, 12, CV_64F, mtm);
CvMat D = cvMat(12, 1, CV_64F, d);
CvMat Ut = cvMat(12, 12, CV_64F, ut);
cvMulTransposed(M, &MtM, 1);
cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
cvReleaseMat(&M);
double l_6x10[6 * 10] = {}, rho[6] = {};
CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
CvMat Rho = cvMat(6, 1, CV_64F, rho);
compute_L_6x10(ut, l_6x10);
compute_rho(rho);
double Betas[4][4] = {}, rep_errors[4] = {};
double Rs[4][3][3] = {}, ts[4][3] = {};
find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
gauss_newton(&L_6x10, &Rho, Betas[1]);
rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
gauss_newton(&L_6x10, &Rho, Betas[2]);
rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
gauss_newton(&L_6x10, &Rho, Betas[3]);
rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);
int N = 1;
if (rep_errors[2] < rep_errors[1]) N = 2;
if (rep_errors[3] < rep_errors[N]) N = 3;
Mat(3, 1, CV_64F, ts[N]).copyTo(t);
Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
}
void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
double R_dst[3][3], double t_dst[3])
{
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R_dst[i][j] = R_src[i][j];
t_dst[i] = t_src[i];
}
}
double epnp::dist2(const double * p1, const double * p2)
{
return
(p1[0] - p2[0]) * (p1[0] - p2[0]) +
(p1[1] - p2[1]) * (p1[1] - p2[1]) +
(p1[2] - p2[2]) * (p1[2] - p2[2]);
}
double epnp::dot(const double * v1, const double * v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
void epnp::estimate_R_and_t(double R[3][3], double t[3])
{
double pc0[3] = {}, pw0[3] = {};
pc0[0] = pc0[1] = pc0[2] = 0.0;
pw0[0] = pw0[1] = pw0[2] = 0.0;
for(int i = 0; i < number_of_correspondences; i++) {
const double * pc = &pcs[3 * i];
const double * pw = &pws[3 * i];
for(int j = 0; j < 3; j++) {
pc0[j] += pc[j];
pw0[j] += pw[j];
}
}
for(int j = 0; j < 3; j++) {
pc0[j] /= number_of_correspondences;
pw0[j] /= number_of_correspondences;
}
double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_v[3 * 3] = {};
CvMat ABt = cvMat(3, 3, CV_64F, abt);
CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
cvSetZero(&ABt);
for(int i = 0; i < number_of_correspondences; i++) {
double * pc = &pcs[3 * i];
double * pw = &pws[3 * i];
for(int j = 0; j < 3; j++) {
abt[3 * j ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
}
}
cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
const double det =
R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
if (det < 0) {
R[2][0] = -R[2][0];
R[2][1] = -R[2][1];
R[2][2] = -R[2][2];
}
t[0] = pc0[0] - dot(R[0], pw0);
t[1] = pc0[1] - dot(R[1], pw0);
t[2] = pc0[2] - dot(R[2], pw0);
}
void epnp::solve_for_sign(void)
{
if (pcs[2] < 0.0) {
for(int i = 0; i < 4; i++)
for(int j = 0; j < 3; j++)
ccs[i][j] = -ccs[i][j];
for(int i = 0; i < number_of_correspondences; i++) {
pcs[3 * i ] = -pcs[3 * i];
pcs[3 * i + 1] = -pcs[3 * i + 1];
pcs[3 * i + 2] = -pcs[3 * i + 2];
}
}
}
double epnp::compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3])
{
compute_ccs(betas, ut);
compute_pcs();
solve_for_sign();
estimate_R_and_t(R, t);
return reprojection_error(R, t);
}
double epnp::reprojection_error(const double R[3][3], const double t[3])
{
double sum2 = 0.0;
for(int i = 0; i < number_of_correspondences; i++) {
double * pw = &pws[3 * i];
double Xc = dot(R[0], pw) + t[0];
double Yc = dot(R[1], pw) + t[1];
double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
double ue = uc + fu * Xc * inv_Zc;
double ve = vc + fv * Yc * inv_Zc;
double u = us[2 * i], v = us[2 * i + 1];
sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
}
return sum2 / number_of_correspondences;
}
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_1 = [B11 B12 B13 B14]
void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
double l_6x4[6 * 4] = {}, b4[4] = {};
CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
CvMat B4 = cvMat(4, 1, CV_64F, b4);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
}
cvSolve(&L_6x4, Rho, &B4, CV_SVD);
if (b4[0] < 0) {
betas[0] = sqrt(-b4[0]);
betas[1] = -b4[1] / betas[0];
betas[2] = -b4[2] / betas[0];
betas[3] = -b4[3] / betas[0];
} else {
betas[0] = sqrt(b4[0]);
betas[1] = b4[1] / betas[0];
betas[2] = b4[2] / betas[0];
betas[3] = b4[3] / betas[0];
}
}
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_2 = [B11 B12 B22 ]
void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
double l_6x3[6 * 3] = {}, b3[3] = {};
CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
CvMat B3 = cvMat(3, 1, CV_64F, b3);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
}
cvSolve(&L_6x3, Rho, &B3, CV_SVD);
if (b3[0] < 0) {
betas[0] = sqrt(-b3[0]);
betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;
} else {
betas[0] = sqrt(b3[0]);
betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;
}
if (b3[1] < 0) betas[0] = -betas[0];
betas[2] = 0.0;
betas[3] = 0.0;
}
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_3 = [B11 B12 B22 B13 B23 ]
void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
double l_6x5[6 * 5] = {}, b5[5] = {};
CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
CvMat B5 = cvMat(5, 1, CV_64F, b5);
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
}
cvSolve(&L_6x5, Rho, &B5, CV_SVD);
if (b5[0] < 0) {
betas[0] = sqrt(-b5[0]);
betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;
} else {
betas[0] = sqrt(b5[0]);
betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;
}
if (b5[1] < 0) betas[0] = -betas[0];
betas[2] = b5[3] / betas[0];
betas[3] = 0.0;
}
void epnp::compute_L_6x10(const double * ut, double * l_6x10)
{
const double * v[4];
v[0] = ut + 12 * 11;
v[1] = ut + 12 * 10;
v[2] = ut + 12 * 9;
v[3] = ut + 12 * 8;
double dv[4][6][3] = {};
for(int i = 0; i < 4; i++) {
int a = 0, b = 1;
for(int j = 0; j < 6; j++) {
dv[i][j][0] = v[i][3 * a ] - v[i][3 * b];
dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
b++;
if (b > 3) {
a++;
b = a + 1;
}
}
}
for(int i = 0; i < 6; i++) {
double * row = l_6x10 + 10 * i;
row[0] = dot(dv[0][i], dv[0][i]);
row[1] = 2.0f * dot(dv[0][i], dv[1][i]);
row[2] = dot(dv[1][i], dv[1][i]);
row[3] = 2.0f * dot(dv[0][i], dv[2][i]);
row[4] = 2.0f * dot(dv[1][i], dv[2][i]);
row[5] = dot(dv[2][i], dv[2][i]);
row[6] = 2.0f * dot(dv[0][i], dv[3][i]);
row[7] = 2.0f * dot(dv[1][i], dv[3][i]);
row[8] = 2.0f * dot(dv[2][i], dv[3][i]);
row[9] = dot(dv[3][i], dv[3][i]);
}
}
void epnp::compute_rho(double * rho)
{
rho[0] = dist2(cws[0], cws[1]);
rho[1] = dist2(cws[0], cws[2]);
rho[2] = dist2(cws[0], cws[3]);
rho[3] = dist2(cws[1], cws[2]);
rho[4] = dist2(cws[1], cws[3]);
rho[5] = dist2(cws[2], cws[3]);
}
void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double betas[4], CvMat * A, CvMat * b)
{
for(int i = 0; i < 6; i++) {
const double * rowL = l_6x10 + i * 10;
double * rowA = A->data.db + i * 4;
rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3];
rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3];
rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3];
rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
cvmSet(b, i, 0, rho[i] -
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[1] * betas[1] +
rowL[3] * betas[0] * betas[2] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
rowL[6] * betas[0] * betas[3] +
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
}
}
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4])
{
const int iterations_number = 5;
double a[6*4] = {}, b[6] = {}, x[4] = {};
CvMat A = cvMat(6, 4, CV_64F, a);
CvMat B = cvMat(6, 1, CV_64F, b);
CvMat X = cvMat(4, 1, CV_64F, x);
for(int k = 0; k < iterations_number; k++)
{
compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
betas, &A, &B);
qr_solve(&A, &B, &X);
for(int i = 0; i < 4; i++)
betas[i] += x[i];
}
}
void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
{
const int nr = A->rows;
const int nc = A->cols;
if (nc <= 0 || nr <= 0)
return;
if (max_nr != 0 && max_nr < nr)
{
delete [] A1;
delete [] A2;
}
if (max_nr < nr)
{
max_nr = nr;
A1 = new double[nr];
A2 = new double[nr];
}
double * pA = A->data.db, * ppAkk = pA;
for(int k = 0; k < nc; k++)
{
double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
for(int i = k + 1; i < nr; i++)
{
double elt = fabs(*ppAik1);
if (eta < elt) eta = elt;
ppAik1 += nc;
}
if (eta == 0)
{
A1[k] = A2[k] = 0.0;
//cerr << "God damnit, A is singular, this shouldn't happen." << endl;
return;
}
else
{
double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++)
{
*ppAik2 *= inv_eta;
sum2 += *ppAik2 * *ppAik2;
ppAik2 += nc;
}
double sigma = sqrt(sum2);
if (*ppAkk < 0)
sigma = -sigma;
*ppAkk += sigma;
A1[k] = sigma * *ppAkk;
A2[k] = -eta * sigma;
for(int j = k + 1; j < nc; j++)
{
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++)
{
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++)
{
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
}
}
ppAkk += nc + 1;
}
// b <- Qt b
double * ppAjj = pA, * pb = b->data.db;
for(int j = 0; j < nc; j++)
{
double * ppAij = ppAjj, tau = 0;
for(int i = j; i < nr; i++)
{
tau += *ppAij * pb[i];
ppAij += nc;
}
tau /= A1[j];
ppAij = ppAjj;
for(int i = j; i < nr; i++)
{
pb[i] -= tau * *ppAij;
ppAij += nc;
}
ppAjj += nc + 1;
}
// X = R-1 b
double * pX = X->data.db;
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--)
{
double * ppAij = pA + i * nc + (i + 1), sum = 0;
for(int j = i + 1; j < nc; j++)
{
sum += *ppAij * pX[j];
ppAij++;
}
pX[i] = (pb[i] - sum) / A2[i];
}
}
}

88
Lib/opencv/sources/modules/calib3d/src/epnp.h Normal file
View File

@@ -0,0 +1,88 @@
#ifndef epnp_h
#define epnp_h
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
namespace cv
{
class epnp {
public:
epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
~epnp();
void add_correspondence(const double X, const double Y, const double Z,
const double u, const double v);
void compute_pose(cv::Mat& R, cv::Mat& t);
private:
epnp(const epnp &); // copy disabled
epnp& operator=(const epnp &); // assign disabled
template <typename T>
void init_camera_parameters(const cv::Mat& cameraMatrix)
{
uc = cameraMatrix.at<T> (0, 2);
vc = cameraMatrix.at<T> (1, 2);
fu = cameraMatrix.at<T> (0, 0);
fv = cameraMatrix.at<T> (1, 1);
}
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
for(int i = 0; i < number_of_correspondences; i++)
{
pws[3 * i ] = opoints.at<OpointType>(i).x;
pws[3 * i + 1] = opoints.at<OpointType>(i).y;
pws[3 * i + 2] = opoints.at<OpointType>(i).z;
us[2 * i ] = ipoints.at<IpointType>(i).x*fu + uc;
us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
}
}
double reprojection_error(const double R[3][3], const double t[3]);
void choose_control_points(void);
void compute_barycentric_coordinates(void);
void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
void compute_ccs(const double * betas, const double * ut);
void compute_pcs(void);
void solve_for_sign(void);
void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void qr_solve(CvMat * A, CvMat * b, CvMat * X);
double dot(const double * v1, const double * v2);
double dist2(const double * p1, const double * p2);
void compute_rho(double * rho);
void compute_L_6x10(const double * ut, double * l_6x10);
void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double cb[4], CvMat * A, CvMat * b);
double compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3]);
void estimate_R_and_t(double R[3][3], double t[3]);
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
double R_src[3][3], double t_src[3]);
double uc, vc, fu, fv;
std::vector<double> pws, us, alphas, pcs;
int number_of_correspondences;
double cws[4][3], ccs[4][3];
int max_nr;
double * A1, * A2;
};
}
#endif

1658
Lib/opencv/sources/modules/calib3d/src/fisheye.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

61
Lib/opencv/sources/modules/calib3d/src/fisheye.hpp Normal file
View File

@@ -0,0 +1,61 @@
#ifndef FISHEYE_INTERNAL_H
#define FISHEYE_INTERNAL_H
#include "precomp.hpp"
namespace cv { namespace internal {
struct CV_EXPORTS IntrinsicParams
{
Vec2d f;
Vec2d c;
Vec4d k;
double alpha;
std::vector<uchar> isEstimate;
IntrinsicParams();
IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
IntrinsicParams operator+(const Mat& a);
IntrinsicParams& operator =(const Mat& a);
void Init(const cv::Vec2d& f, const cv::Vec2d& c, const cv::Vec4d& k = Vec4d(0,0,0,0), const double& alpha = 0);
};
void projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
cv::InputArray _rvec,cv::InputArray _tvec,
const IntrinsicParams& param, cv::OutputArray jacobian);
void ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
Mat& tvec, Mat& J, const int MaxIter,
const IntrinsicParams& param, const double thresh_cond);
CV_EXPORTS Mat ComputeHomography(Mat m, Mat M);
CV_EXPORTS Mat NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param);
void InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk);
void CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
const IntrinsicParams& param, const int check_cond,
const double thresh_cond, InputOutputArray omc, InputOutputArray Tc);
void ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
const IntrinsicParams& param, InputArray omc, InputArray Tc,
const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3);
CV_EXPORTS void EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
const IntrinsicParams& params, InputArray omc, InputArray Tc,
IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms);
void dAB(cv::InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB);
void JRodriguesMatlab(const Mat& src, Mat& dst);
void compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2,
Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2,
Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2);
double median(const Mat& row);
Vec3d median3d(InputArray m);
}}
#endif

668
Lib/opencv/sources/modules/calib3d/src/five-point.cpp Normal file
View File

@@ -0,0 +1,668 @@
/* This is a 5-point algorithm contributed to OpenCV by the author, Bo Li.
It implements the 5-point algorithm solver from Nister's paper:
Nister, An efficient solution to the five-point relative pose problem, PAMI, 2004.
*/
/* Copyright (c) 2013, Bo Li (prclibo@gmail.com), ETH Zurich
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "precomp.hpp"
namespace cv
{
class EMEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
{
public:
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
{
Mat q1 = _m1.getMat(), q2 = _m2.getMat();
Mat Q1 = q1.reshape(1, (int)q1.total());
Mat Q2 = q2.reshape(1, (int)q2.total());
int n = Q1.rows;
Mat Q(n, 9, CV_64F);
Q.col(0) = Q1.col(0).mul( Q2.col(0) );
Q.col(1) = Q1.col(1).mul( Q2.col(0) );
Q.col(2) = Q2.col(0) * 1.0;
Q.col(3) = Q1.col(0).mul( Q2.col(1) );
Q.col(4) = Q1.col(1).mul( Q2.col(1) );
Q.col(5) = Q2.col(1) * 1.0;
Q.col(6) = Q1.col(0) * 1.0;
Q.col(7) = Q1.col(1) * 1.0;
Q.col(8) = 1.0;
Mat U, W, Vt;
SVD::compute(Q, W, U, Vt, SVD::MODIFY_A | SVD::FULL_UV);
Mat EE = Mat(Vt.t()).colRange(5, 9) * 1.0;
Mat A(10, 20, CV_64F);
EE = EE.t();
getCoeffMat(EE.ptr<double>(), A.ptr<double>());
EE = EE.t();
A = A.colRange(0, 10).inv() * A.colRange(10, 20);
double b[3 * 13];
Mat B(3, 13, CV_64F, b);
for (int i = 0; i < 3; i++)
{
Mat arow1 = A.row(i * 2 + 4) * 1.0;
Mat arow2 = A.row(i * 2 + 5) * 1.0;
Mat row1(1, 13, CV_64F, Scalar(0.0));
Mat row2(1, 13, CV_64F, Scalar(0.0));
row1.colRange(1, 4) = arow1.colRange(0, 3) * 1.0;
row1.colRange(5, 8) = arow1.colRange(3, 6) * 1.0;
row1.colRange(9, 13) = arow1.colRange(6, 10) * 1.0;
row2.colRange(0, 3) = arow2.colRange(0, 3) * 1.0;
row2.colRange(4, 7) = arow2.colRange(3, 6) * 1.0;
row2.colRange(8, 12) = arow2.colRange(6, 10) * 1.0;
B.row(i) = row1 - row2;
}
double c[11];
Mat coeffs(1, 11, CV_64F, c);
c[10] = (b[0]*b[17]*b[34]+b[26]*b[4]*b[21]-b[26]*b[17]*b[8]-b[13]*b[4]*b[34]-b[0]*b[21]*b[30]+b[13]*b[30]*b[8]);
c[9] = (b[26]*b[4]*b[22]+b[14]*b[30]*b[8]+b[13]*b[31]*b[8]+b[1]*b[17]*b[34]-b[13]*b[5]*b[34]+b[26]*b[5]*b[21]-b[0]*b[21]*b[31]-b[26]*b[17]*b[9]-b[1]*b[21]*b[30]+b[27]*b[4]*b[21]+b[0]*b[17]*b[35]-b[0]*b[22]*b[30]+b[13]*b[30]*b[9]+b[0]*b[18]*b[34]-b[27]*b[17]*b[8]-b[14]*b[4]*b[34]-b[13]*b[4]*b[35]-b[26]*b[18]*b[8]);
c[8] = (b[14]*b[30]*b[9]+b[14]*b[31]*b[8]+b[13]*b[31]*b[9]-b[13]*b[4]*b[36]-b[13]*b[5]*b[35]+b[15]*b[30]*b[8]-b[13]*b[6]*b[34]+b[13]*b[30]*b[10]+b[13]*b[32]*b[8]-b[14]*b[4]*b[35]-b[14]*b[5]*b[34]+b[26]*b[4]*b[23]+b[26]*b[5]*b[22]+b[26]*b[6]*b[21]-b[26]*b[17]*b[10]-b[15]*b[4]*b[34]-b[26]*b[18]*b[9]-b[26]*b[19]*b[8]+b[27]*b[4]*b[22]+b[27]*b[5]*b[21]-b[27]*b[17]*b[9]-b[27]*b[18]*b[8]-b[1]*b[21]*b[31]-b[0]*b[23]*b[30]-b[0]*b[21]*b[32]+b[28]*b[4]*b[21]-b[28]*b[17]*b[8]+b[2]*b[17]*b[34]+b[0]*b[18]*b[35]-b[0]*b[22]*b[31]+b[0]*b[17]*b[36]+b[0]*b[19]*b[34]-b[1]*b[22]*b[30]+b[1]*b[18]*b[34]+b[1]*b[17]*b[35]-b[2]*b[21]*b[30]);
c[7] = (b[14]*b[30]*b[10]+b[14]*b[32]*b[8]-b[3]*b[21]*b[30]+b[3]*b[17]*b[34]+b[13]*b[32]*b[9]+b[13]*b[33]*b[8]-b[13]*b[4]*b[37]-b[13]*b[5]*b[36]+b[15]*b[30]*b[9]+b[15]*b[31]*b[8]-b[16]*b[4]*b[34]-b[13]*b[6]*b[35]-b[13]*b[7]*b[34]+b[13]*b[30]*b[11]+b[13]*b[31]*b[10]+b[14]*b[31]*b[9]-b[14]*b[4]*b[36]-b[14]*b[5]*b[35]-b[14]*b[6]*b[34]+b[16]*b[30]*b[8]-b[26]*b[20]*b[8]+b[26]*b[4]*b[24]+b[26]*b[5]*b[23]+b[26]*b[6]*b[22]+b[26]*b[7]*b[21]-b[26]*b[17]*b[11]-b[15]*b[4]*b[35]-b[15]*b[5]*b[34]-b[26]*b[18]*b[10]-b[26]*b[19]*b[9]+b[27]*b[4]*b[23]+b[27]*b[5]*b[22]+b[27]*b[6]*b[21]-b[27]*b[17]*b[10]-b[27]*b[18]*b[9]-b[27]*b[19]*b[8]+b[0]*b[17]*b[37]-b[0]*b[23]*b[31]-b[0]*b[24]*b[30]-b[0]*b[21]*b[33]-b[29]*b[17]*b[8]+b[28]*b[4]*b[22]+b[28]*b[5]*b[21]-b[28]*b[17]*b[9]-b[28]*b[18]*b[8]+b[29]*b[4]*b[21]+b[1]*b[19]*b[34]-b[2]*b[21]*b[31]+b[0]*b[20]*b[34]+b[0]*b[19]*b[35]+b[0]*b[18]*b[36]-b[0]*b[22]*b[32]-b[1]*b[23]*b[30]-b[1]*b[21]*b[32]+b[1]*b[18]*b[35]-b[1]*b[22]*b[31]-b[2]*b[22]*b[30]+b[2]*b[17]*b[35]+b[1]*b[17]*b[36]+b[2]*b[18]*b[34]);
c[6] = (-b[14]*b[6]*b[35]-b[14]*b[7]*b[34]-b[3]*b[22]*b[30]-b[3]*b[21]*b[31]+b[3]*b[17]*b[35]+b[3]*b[18]*b[34]+b[13]*b[32]*b[10]+b[13]*b[33]*b[9]-b[13]*b[4]*b[38]-b[13]*b[5]*b[37]-b[15]*b[6]*b[34]+b[15]*b[30]*b[10]+b[15]*b[32]*b[8]-b[16]*b[4]*b[35]-b[13]*b[6]*b[36]-b[13]*b[7]*b[35]+b[13]*b[31]*b[11]+b[13]*b[30]*b[12]+b[14]*b[32]*b[9]+b[14]*b[33]*b[8]-b[14]*b[4]*b[37]-b[14]*b[5]*b[36]+b[16]*b[30]*b[9]+b[16]*b[31]*b[8]-b[26]*b[20]*b[9]+b[26]*b[4]*b[25]+b[26]*b[5]*b[24]+b[26]*b[6]*b[23]+b[26]*b[7]*b[22]-b[26]*b[17]*b[12]+b[14]*b[30]*b[11]+b[14]*b[31]*b[10]+b[15]*b[31]*b[9]-b[15]*b[4]*b[36]-b[15]*b[5]*b[35]-b[26]*b[18]*b[11]-b[26]*b[19]*b[10]-b[27]*b[20]*b[8]+b[27]*b[4]*b[24]+b[27]*b[5]*b[23]+b[27]*b[6]*b[22]+b[27]*b[7]*b[21]-b[27]*b[17]*b[11]-b[27]*b[18]*b[10]-b[27]*b[19]*b[9]-b[16]*b[5]*b[34]-b[29]*b[17]*b[9]-b[29]*b[18]*b[8]+b[28]*b[4]*b[23]+b[28]*b[5]*b[22]+b[28]*b[6]*b[21]-b[28]*b[17]*b[10]-b[28]*b[18]*b[9]-b[28]*b[19]*b[8]+b[29]*b[4]*b[22]+b[29]*b[5]*b[21]-b[2]*b[23]*b[30]+b[2]*b[18]*b[35]-b[1]*b[22]*b[32]-b[2]*b[21]*b[32]+b[2]*b[19]*b[34]+b[0]*b[19]*b[36]-b[0]*b[22]*b[33]+b[0]*b[20]*b[35]-b[0]*b[23]*b[32]-b[0]*b[25]*b[30]+b[0]*b[17]*b[38]+b[0]*b[18]*b[37]-b[0]*b[24]*b[31]+b[1]*b[17]*b[37]-b[1]*b[23]*b[31]-b[1]*b[24]*b[30]-b[1]*b[21]*b[33]+b[1]*b[20]*b[34]+b[1]*b[19]*b[35]+b[1]*b[18]*b[36]+b[2]*b[17]*b[36]-b[2]*b[22]*b[31]);
c[5] = (-b[14]*b[6]*b[36]-b[14]*b[7]*b[35]+b[14]*b[31]*b[11]-b[3]*b[23]*b[30]-b[3]*b[21]*b[32]+b[3]*b[18]*b[35]-b[3]*b[22]*b[31]+b[3]*b[17]*b[36]+b[3]*b[19]*b[34]+b[13]*b[32]*b[11]+b[13]*b[33]*b[10]-b[13]*b[5]*b[38]-b[15]*b[6]*b[35]-b[15]*b[7]*b[34]+b[15]*b[30]*b[11]+b[15]*b[31]*b[10]+b[16]*b[31]*b[9]-b[13]*b[6]*b[37]-b[13]*b[7]*b[36]+b[13]*b[31]*b[12]+b[14]*b[32]*b[10]+b[14]*b[33]*b[9]-b[14]*b[4]*b[38]-b[14]*b[5]*b[37]-b[16]*b[6]*b[34]+b[16]*b[30]*b[10]+b[16]*b[32]*b[8]-b[26]*b[20]*b[10]+b[26]*b[5]*b[25]+b[26]*b[6]*b[24]+b[26]*b[7]*b[23]+b[14]*b[30]*b[12]+b[15]*b[32]*b[9]+b[15]*b[33]*b[8]-b[15]*b[4]*b[37]-b[15]*b[5]*b[36]+b[29]*b[5]*b[22]+b[29]*b[6]*b[21]-b[26]*b[18]*b[12]-b[26]*b[19]*b[11]-b[27]*b[20]*b[9]+b[27]*b[4]*b[25]+b[27]*b[5]*b[24]+b[27]*b[6]*b[23]+b[27]*b[7]*b[22]-b[27]*b[17]*b[12]-b[27]*b[18]*b[11]-b[27]*b[19]*b[10]-b[28]*b[20]*b[8]-b[16]*b[4]*b[36]-b[16]*b[5]*b[35]-b[29]*b[17]*b[10]-b[29]*b[18]*b[9]-b[29]*b[19]*b[8]+b[28]*b[4]*b[24]+b[28]*b[5]*b[23]+b[28]*b[6]*b[22]+b[28]*b[7]*b[21]-b[28]*b[17]*b[11]-b[28]*b[18]*b[10]-b[28]*b[19]*b[9]+b[29]*b[4]*b[23]-b[2]*b[22]*b[32]-b[2]*b[21]*b[33]-b[1]*b[24]*b[31]+b[0]*b[18]*b[38]-b[0]*b[24]*b[32]+b[0]*b[19]*b[37]+b[0]*b[20]*b[36]-b[0]*b[25]*b[31]-b[0]*b[23]*b[33]+b[1]*b[19]*b[36]-b[1]*b[22]*b[33]+b[1]*b[20]*b[35]+b[2]*b[19]*b[35]-b[2]*b[24]*b[30]-b[2]*b[23]*b[31]+b[2]*b[20]*b[34]+b[2]*b[17]*b[37]-b[1]*b[25]*b[30]+b[1]*b[18]*b[37]+b[1]*b[17]*b[38]-b[1]*b[23]*b[32]+b[2]*b[18]*b[36]);
c[4] = (-b[14]*b[6]*b[37]-b[14]*b[7]*b[36]+b[14]*b[31]*b[12]+b[3]*b[17]*b[37]-b[3]*b[23]*b[31]-b[3]*b[24]*b[30]-b[3]*b[21]*b[33]+b[3]*b[20]*b[34]+b[3]*b[19]*b[35]+b[3]*b[18]*b[36]-b[3]*b[22]*b[32]+b[13]*b[32]*b[12]+b[13]*b[33]*b[11]-b[15]*b[6]*b[36]-b[15]*b[7]*b[35]+b[15]*b[31]*b[11]+b[15]*b[30]*b[12]+b[16]*b[32]*b[9]+b[16]*b[33]*b[8]-b[13]*b[6]*b[38]-b[13]*b[7]*b[37]+b[14]*b[32]*b[11]+b[14]*b[33]*b[10]-b[14]*b[5]*b[38]-b[16]*b[6]*b[35]-b[16]*b[7]*b[34]+b[16]*b[30]*b[11]+b[16]*b[31]*b[10]-b[26]*b[19]*b[12]-b[26]*b[20]*b[11]+b[26]*b[6]*b[25]+b[26]*b[7]*b[24]+b[15]*b[32]*b[10]+b[15]*b[33]*b[9]-b[15]*b[4]*b[38]-b[15]*b[5]*b[37]+b[29]*b[5]*b[23]+b[29]*b[6]*b[22]+b[29]*b[7]*b[21]-b[27]*b[20]*b[10]+b[27]*b[5]*b[25]+b[27]*b[6]*b[24]+b[27]*b[7]*b[23]-b[27]*b[18]*b[12]-b[27]*b[19]*b[11]-b[28]*b[20]*b[9]-b[16]*b[4]*b[37]-b[16]*b[5]*b[36]+b[0]*b[19]*b[38]-b[0]*b[24]*b[33]+b[0]*b[20]*b[37]-b[29]*b[17]*b[11]-b[29]*b[18]*b[10]-b[29]*b[19]*b[9]+b[28]*b[4]*b[25]+b[28]*b[5]*b[24]+b[28]*b[6]*b[23]+b[28]*b[7]*b[22]-b[28]*b[17]*b[12]-b[28]*b[18]*b[11]-b[28]*b[19]*b[10]-b[29]*b[20]*b[8]+b[29]*b[4]*b[24]+b[2]*b[18]*b[37]-b[0]*b[25]*b[32]+b[1]*b[18]*b[38]-b[1]*b[24]*b[32]+b[1]*b[19]*b[37]+b[1]*b[20]*b[36]-b[1]*b[25]*b[31]+b[2]*b[17]*b[38]+b[2]*b[19]*b[36]-b[2]*b[24]*b[31]-b[2]*b[22]*b[33]-b[2]*b[23]*b[32]+b[2]*b[20]*b[35]-b[1]*b[23]*b[33]-b[2]*b[25]*b[30]);
c[3] = (-b[14]*b[6]*b[38]-b[14]*b[7]*b[37]+b[3]*b[19]*b[36]-b[3]*b[22]*b[33]+b[3]*b[20]*b[35]-b[3]*b[23]*b[32]-b[3]*b[25]*b[30]+b[3]*b[17]*b[38]+b[3]*b[18]*b[37]-b[3]*b[24]*b[31]-b[15]*b[6]*b[37]-b[15]*b[7]*b[36]+b[15]*b[31]*b[12]+b[16]*b[32]*b[10]+b[16]*b[33]*b[9]+b[13]*b[33]*b[12]-b[13]*b[7]*b[38]+b[14]*b[32]*b[12]+b[14]*b[33]*b[11]-b[16]*b[6]*b[36]-b[16]*b[7]*b[35]+b[16]*b[31]*b[11]+b[16]*b[30]*b[12]+b[15]*b[32]*b[11]+b[15]*b[33]*b[10]-b[15]*b[5]*b[38]+b[29]*b[5]*b[24]+b[29]*b[6]*b[23]-b[26]*b[20]*b[12]+b[26]*b[7]*b[25]-b[27]*b[19]*b[12]-b[27]*b[20]*b[11]+b[27]*b[6]*b[25]+b[27]*b[7]*b[24]-b[28]*b[20]*b[10]-b[16]*b[4]*b[38]-b[16]*b[5]*b[37]+b[29]*b[7]*b[22]-b[29]*b[17]*b[12]-b[29]*b[18]*b[11]-b[29]*b[19]*b[10]+b[28]*b[5]*b[25]+b[28]*b[6]*b[24]+b[28]*b[7]*b[23]-b[28]*b[18]*b[12]-b[28]*b[19]*b[11]-b[29]*b[20]*b[9]+b[29]*b[4]*b[25]-b[2]*b[24]*b[32]+b[0]*b[20]*b[38]-b[0]*b[25]*b[33]+b[1]*b[19]*b[38]-b[1]*b[24]*b[33]+b[1]*b[20]*b[37]-b[2]*b[25]*b[31]+b[2]*b[20]*b[36]-b[1]*b[25]*b[32]+b[2]*b[19]*b[37]+b[2]*b[18]*b[38]-b[2]*b[23]*b[33]);
c[2] = (b[3]*b[18]*b[38]-b[3]*b[24]*b[32]+b[3]*b[19]*b[37]+b[3]*b[20]*b[36]-b[3]*b[25]*b[31]-b[3]*b[23]*b[33]-b[15]*b[6]*b[38]-b[15]*b[7]*b[37]+b[16]*b[32]*b[11]+b[16]*b[33]*b[10]-b[16]*b[5]*b[38]-b[16]*b[6]*b[37]-b[16]*b[7]*b[36]+b[16]*b[31]*b[12]+b[14]*b[33]*b[12]-b[14]*b[7]*b[38]+b[15]*b[32]*b[12]+b[15]*b[33]*b[11]+b[29]*b[5]*b[25]+b[29]*b[6]*b[24]-b[27]*b[20]*b[12]+b[27]*b[7]*b[25]-b[28]*b[19]*b[12]-b[28]*b[20]*b[11]+b[29]*b[7]*b[23]-b[29]*b[18]*b[12]-b[29]*b[19]*b[11]+b[28]*b[6]*b[25]+b[28]*b[7]*b[24]-b[29]*b[20]*b[10]+b[2]*b[19]*b[38]-b[1]*b[25]*b[33]+b[2]*b[20]*b[37]-b[2]*b[24]*b[33]-b[2]*b[25]*b[32]+b[1]*b[20]*b[38]);
c[1] = (b[29]*b[7]*b[24]-b[29]*b[20]*b[11]+b[2]*b[20]*b[38]-b[2]*b[25]*b[33]-b[28]*b[20]*b[12]+b[28]*b[7]*b[25]-b[29]*b[19]*b[12]-b[3]*b[24]*b[33]+b[15]*b[33]*b[12]+b[3]*b[19]*b[38]-b[16]*b[6]*b[38]+b[3]*b[20]*b[37]+b[16]*b[32]*b[12]+b[29]*b[6]*b[25]-b[16]*b[7]*b[37]-b[3]*b[25]*b[32]-b[15]*b[7]*b[38]+b[16]*b[33]*b[11]);
c[0] = -b[29]*b[20]*b[12]+b[29]*b[7]*b[25]+b[16]*b[33]*b[12]-b[16]*b[7]*b[38]+b[3]*b[20]*b[38]-b[3]*b[25]*b[33];
std::vector<Complex<double> > roots;
solvePoly(coeffs, roots);
std::vector<double> xs, ys, zs;
int count = 0;
Mat ematrix(10*3, 3, CV_64F);
double* e = ematrix.ptr<double>();
for (size_t i = 0; i < roots.size(); i++)
{
if (fabs(roots[i].im) > 1e-10) continue;
double z1 = roots[i].re;
double z2 = z1 * z1;
double z3 = z2 * z1;
double z4 = z3 * z1;
double bz[3][3];
for (int j = 0; j < 3; j++)
{
const double * br = b + j * 13;
bz[j][0] = br[0] * z3 + br[1] * z2 + br[2] * z1 + br[3];
bz[j][1] = br[4] * z3 + br[5] * z2 + br[6] * z1 + br[7];
bz[j][2] = br[8] * z4 + br[9] * z3 + br[10] * z2 + br[11] * z1 + br[12];
}
Mat Bz(3, 3, CV_64F, bz);
cv::Mat xy1;
SVD::solveZ(Bz, xy1);
if (fabs(xy1.at<double>(2)) < 1e-10) continue;
xs.push_back(xy1.at<double>(0) / xy1.at<double>(2));
ys.push_back(xy1.at<double>(1) / xy1.at<double>(2));
zs.push_back(z1);
cv::Mat Evec = EE.col(0) * xs.back() + EE.col(1) * ys.back() + EE.col(2) * zs.back() + EE.col(3);
Evec /= norm(Evec);
memcpy(e + count * 9, Evec.ptr(), 9 * sizeof(double));
count++;
}
ematrix.rowRange(0, count*3).copyTo(_model);
return count;
}
protected:
void getCoeffMat(double *e, double *A) const
{
double ep2[36], ep3[36];
for (int i = 0; i < 36; i++)
{
ep2[i] = e[i] * e[i];
ep3[i] = ep2[i] * e[i];
}
A[0]=e[33]*e[28]*e[32]-e[33]*e[31]*e[29]+e[30]*e[34]*e[29]-e[30]*e[28]*e[35]-e[27]*e[32]*e[34]+e[27]*e[31]*e[35];
A[146]=.5000000000*e[6]*ep2[8]-.5000000000*e[6]*ep2[5]+.5000000000*ep3[6]+.5000000000*e[6]*ep2[7]-.5000000000*e[6]*ep2[4]+e[0]*e[2]*e[8]+e[3]*e[4]*e[7]+e[3]*e[5]*e[8]+e[0]*e[1]*e[7]-.5000000000*e[6]*ep2[1]-.5000000000*e[6]*ep2[2]+.5000000000*ep2[0]*e[6]+.5000000000*ep2[3]*e[6];
A[1]=e[30]*e[34]*e[2]+e[33]*e[1]*e[32]-e[3]*e[28]*e[35]+e[0]*e[31]*e[35]+e[3]*e[34]*e[29]-e[30]*e[1]*e[35]+e[27]*e[31]*e[8]-e[27]*e[32]*e[7]-e[30]*e[28]*e[8]-e[33]*e[31]*e[2]-e[0]*e[32]*e[34]+e[6]*e[28]*e[32]-e[33]*e[4]*e[29]+e[33]*e[28]*e[5]+e[30]*e[7]*e[29]+e[27]*e[4]*e[35]-e[27]*e[5]*e[34]-e[6]*e[31]*e[29];
A[147]=e[9]*e[27]*e[15]+e[9]*e[29]*e[17]+e[9]*e[11]*e[35]+e[9]*e[28]*e[16]+e[9]*e[10]*e[34]+e[27]*e[11]*e[17]+e[27]*e[10]*e[16]+e[12]*e[30]*e[15]+e[12]*e[32]*e[17]+e[12]*e[14]*e[35]+e[12]*e[31]*e[16]+e[12]*e[13]*e[34]+e[30]*e[14]*e[17]+e[30]*e[13]*e[16]+e[15]*e[35]*e[17]+e[15]*e[34]*e[16]-1.*e[15]*e[28]*e[10]-1.*e[15]*e[31]*e[13]-1.*e[15]*e[32]*e[14]-1.*e[15]*e[29]*e[11]+.5000000000*ep2[9]*e[33]+.5000000000*e[33]*ep2[16]-.5000000000*e[33]*ep2[11]+.5000000000*e[33]*ep2[12]+1.500000000*e[33]*ep2[15]+.5000000000*e[33]*ep2[17]-.5000000000*e[33]*ep2[10]-.5000000000*e[33]*ep2[14]-.5000000000*e[33]*ep2[13];
A[2]=-e[33]*e[22]*e[29]-e[33]*e[31]*e[20]-e[27]*e[32]*e[25]+e[27]*e[22]*e[35]-e[27]*e[23]*e[34]+e[27]*e[31]*e[26]+e[33]*e[28]*e[23]-e[21]*e[28]*e[35]+e[30]*e[25]*e[29]+e[24]*e[28]*e[32]-e[24]*e[31]*e[29]+e[18]*e[31]*e[35]-e[30]*e[28]*e[26]-e[30]*e[19]*e[35]+e[21]*e[34]*e[29]+e[33]*e[19]*e[32]-e[18]*e[32]*e[34]+e[30]*e[34]*e[20];
A[144]=e[18]*e[2]*e[17]+e[3]*e[21]*e[15]+e[3]*e[12]*e[24]+e[3]*e[23]*e[17]+e[3]*e[14]*e[26]+e[3]*e[22]*e[16]+e[3]*e[13]*e[25]+3.*e[6]*e[24]*e[15]+e[6]*e[26]*e[17]+e[6]*e[25]*e[16]+e[0]*e[20]*e[17]+e[0]*e[11]*e[26]+e[0]*e[19]*e[16]+e[0]*e[10]*e[25]+e[15]*e[26]*e[8]-1.*e[15]*e[20]*e[2]-1.*e[15]*e[19]*e[1]-1.*e[15]*e[22]*e[4]+e[15]*e[25]*e[7]-1.*e[15]*e[23]*e[5]+e[12]*e[21]*e[6]+e[12]*e[22]*e[7]+e[12]*e[4]*e[25]+e[12]*e[23]*e[8]+e[12]*e[5]*e[26]-1.*e[24]*e[11]*e[2]-1.*e[24]*e[10]*e[1]-1.*e[24]*e[13]*e[4]+e[24]*e[16]*e[7]-1.*e[24]*e[14]*e[5]+e[24]*e[17]*e[8]+e[21]*e[13]*e[7]+e[21]*e[4]*e[16]+e[21]*e[14]*e[8]+e[21]*e[5]*e[17]-1.*e[6]*e[23]*e[14]-1.*e[6]*e[20]*e[11]-1.*e[6]*e[19]*e[10]-1.*e[6]*e[22]*e[13]+e[9]*e[18]*e[6]+e[9]*e[0]*e[24]+e[9]*e[19]*e[7]+e[9]*e[1]*e[25]+e[9]*e[20]*e[8]+e[9]*e[2]*e[26]+e[18]*e[0]*e[15]+e[18]*e[10]*e[7]+e[18]*e[1]*e[16]+e[18]*e[11]*e[8];
A[3]=e[33]*e[10]*e[32]+e[33]*e[28]*e[14]-e[33]*e[13]*e[29]-e[33]*e[31]*e[11]+e[9]*e[31]*e[35]-e[9]*e[32]*e[34]+e[27]*e[13]*e[35]-e[27]*e[32]*e[16]+e[27]*e[31]*e[17]-e[27]*e[14]*e[34]+e[12]*e[34]*e[29]-e[12]*e[28]*e[35]+e[30]*e[34]*e[11]+e[30]*e[16]*e[29]-e[30]*e[10]*e[35]-e[30]*e[28]*e[17]+e[15]*e[28]*e[32]-e[15]*e[31]*e[29];
A[145]=e[0]*e[27]*e[6]+e[0]*e[28]*e[7]+e[0]*e[1]*e[34]+e[0]*e[29]*e[8]+e[0]*e[2]*e[35]+e[6]*e[34]*e[7]-1.*e[6]*e[32]*e[5]+e[6]*e[30]*e[3]+e[6]*e[35]*e[8]-1.*e[6]*e[29]*e[2]-1.*e[6]*e[28]*e[1]-1.*e[6]*e[31]*e[4]+e[27]*e[1]*e[7]+e[27]*e[2]*e[8]+e[3]*e[31]*e[7]+e[3]*e[4]*e[34]+e[3]*e[32]*e[8]+e[3]*e[5]*e[35]+e[30]*e[4]*e[7]+e[30]*e[5]*e[8]+.5000000000*ep2[0]*e[33]+1.500000000*e[33]*ep2[6]-.5000000000*e[33]*ep2[4]-.5000000000*e[33]*ep2[5]-.5000000000*e[33]*ep2[1]+.5000000000*e[33]*ep2[7]+.5000000000*e[33]*ep2[3]-.5000000000*e[33]*ep2[2]+.5000000000*e[33]*ep2[8];
A[4]=-e[0]*e[23]*e[16]+e[9]*e[4]*e[26]+e[9]*e[22]*e[8]-e[9]*e[5]*e[25]-e[9]*e[23]*e[7]+e[18]*e[4]*e[17]+e[18]*e[13]*e[8]-e[18]*e[5]*e[16]-e[18]*e[14]*e[7]+e[3]*e[16]*e[20]+e[3]*e[25]*e[11]-e[3]*e[10]*e[26]-e[3]*e[19]*e[17]+e[12]*e[7]*e[20]+e[12]*e[25]*e[2]-e[12]*e[1]*e[26]-e[12]*e[19]*e[8]+e[21]*e[7]*e[11]+e[21]*e[16]*e[2]-e[21]*e[1]*e[17]-e[21]*e[10]*e[8]+e[6]*e[10]*e[23]+e[6]*e[19]*e[14]-e[6]*e[13]*e[20]-e[6]*e[22]*e[11]+e[15]*e[1]*e[23]+e[15]*e[19]*e[5]-e[15]*e[4]*e[20]-e[15]*e[22]*e[2]+e[24]*e[1]*e[14]+e[24]*e[10]*e[5]-e[24]*e[4]*e[11]-e[24]*e[13]*e[2]+e[0]*e[13]*e[26]+e[0]*e[22]*e[17]-e[0]*e[14]*e[25];
A[150]=e[18]*e[19]*e[25]+.5000000000*ep3[24]-.5000000000*e[24]*ep2[23]+e[18]*e[20]*e[26]+e[21]*e[22]*e[25]+e[21]*e[23]*e[26]-.5000000000*e[24]*ep2[19]+.5000000000*ep2[21]*e[24]+.5000000000*e[24]*ep2[26]-.5000000000*e[24]*ep2[20]+.5000000000*ep2[18]*e[24]-.5000000000*e[24]*ep2[22]+.5000000000*e[24]*ep2[25];
A[5]=-e[3]*e[1]*e[35]-e[0]*e[32]*e[7]+e[27]*e[4]*e[8]+e[33]*e[1]*e[5]-e[33]*e[4]*e[2]+e[0]*e[4]*e[35]+e[3]*e[34]*e[2]-e[30]*e[1]*e[8]+e[30]*e[7]*e[2]-e[6]*e[4]*e[29]+e[3]*e[7]*e[29]+e[6]*e[1]*e[32]-e[0]*e[5]*e[34]-e[3]*e[28]*e[8]+e[0]*e[31]*e[8]+e[6]*e[28]*e[5]-e[6]*e[31]*e[2]-e[27]*e[5]*e[7];
A[151]=e[33]*e[16]*e[7]-1.*e[33]*e[14]*e[5]+e[33]*e[17]*e[8]+e[30]*e[13]*e[7]+e[30]*e[4]*e[16]+e[30]*e[14]*e[8]+e[30]*e[5]*e[17]+e[6]*e[27]*e[9]-1.*e[6]*e[28]*e[10]-1.*e[6]*e[31]*e[13]-1.*e[6]*e[32]*e[14]-1.*e[6]*e[29]*e[11]+e[9]*e[28]*e[7]+e[9]*e[1]*e[34]+e[9]*e[29]*e[8]+e[9]*e[2]*e[35]+e[27]*e[10]*e[7]+e[27]*e[1]*e[16]+e[27]*e[11]*e[8]+e[27]*e[2]*e[17]+e[3]*e[30]*e[15]+e[3]*e[12]*e[33]+e[3]*e[32]*e[17]+e[3]*e[14]*e[35]+e[3]*e[31]*e[16]+e[3]*e[13]*e[34]+3.*e[6]*e[33]*e[15]+e[6]*e[35]*e[17]+e[6]*e[34]*e[16]+e[0]*e[27]*e[15]+e[0]*e[9]*e[33]+e[0]*e[29]*e[17]+e[0]*e[11]*e[35]+e[0]*e[28]*e[16]+e[0]*e[10]*e[34]+e[15]*e[34]*e[7]-1.*e[15]*e[32]*e[5]+e[15]*e[35]*e[8]-1.*e[15]*e[29]*e[2]-1.*e[15]*e[28]*e[1]-1.*e[15]*e[31]*e[4]+e[12]*e[30]*e[6]+e[12]*e[31]*e[7]+e[12]*e[4]*e[34]+e[12]*e[32]*e[8]+e[12]*e[5]*e[35]-1.*e[33]*e[11]*e[2]-1.*e[33]*e[10]*e[1]-1.*e[33]*e[13]*e[4];
A[6]=e[6]*e[1]*e[5]-e[6]*e[4]*e[2]+e[3]*e[7]*e[2]+e[0]*e[4]*e[8]-e[0]*e[5]*e[7]-e[3]*e[1]*e[8];
A[148]=.5000000000*ep3[15]+e[9]*e[10]*e[16]-.5000000000*e[15]*ep2[11]+e[9]*e[11]*e[17]+.5000000000*ep2[12]*e[15]+.5000000000*e[15]*ep2[16]+.5000000000*e[15]*ep2[17]-.5000000000*e[15]*ep2[13]+.5000000000*ep2[9]*e[15]+e[12]*e[14]*e[17]-.5000000000*e[15]*ep2[10]-.5000000000*e[15]*ep2[14]+e[12]*e[13]*e[16];
A[7]=e[15]*e[28]*e[14]-e[15]*e[13]*e[29]-e[15]*e[31]*e[11]+e[33]*e[10]*e[14]-e[33]*e[13]*e[11]+e[9]*e[13]*e[35]-e[9]*e[32]*e[16]+e[9]*e[31]*e[17]-e[9]*e[14]*e[34]+e[27]*e[13]*e[17]-e[27]*e[14]*e[16]+e[12]*e[34]*e[11]+e[12]*e[16]*e[29]-e[12]*e[10]*e[35]-e[12]*e[28]*e[17]+e[30]*e[16]*e[11]-e[30]*e[10]*e[17]+e[15]*e[10]*e[32];
A[149]=e[18]*e[27]*e[24]+e[18]*e[28]*e[25]+e[18]*e[19]*e[34]+e[18]*e[29]*e[26]+e[18]*e[20]*e[35]+e[27]*e[19]*e[25]+e[27]*e[20]*e[26]+e[21]*e[30]*e[24]+e[21]*e[31]*e[25]+e[21]*e[22]*e[34]+e[21]*e[32]*e[26]+e[21]*e[23]*e[35]+e[30]*e[22]*e[25]+e[30]*e[23]*e[26]+e[24]*e[34]*e[25]+e[24]*e[35]*e[26]-1.*e[24]*e[29]*e[20]-1.*e[24]*e[31]*e[22]-1.*e[24]*e[32]*e[23]-1.*e[24]*e[28]*e[19]+1.500000000*e[33]*ep2[24]+.5000000000*e[33]*ep2[25]+.5000000000*e[33]*ep2[26]-.5000000000*e[33]*ep2[23]-.5000000000*e[33]*ep2[19]-.5000000000*e[33]*ep2[20]-.5000000000*e[33]*ep2[22]+.5000000000*ep2[18]*e[33]+.5000000000*ep2[21]*e[33];
A[9]=e[21]*e[25]*e[29]-e[27]*e[23]*e[25]+e[24]*e[19]*e[32]-e[21]*e[28]*e[26]-e[21]*e[19]*e[35]+e[18]*e[31]*e[26]-e[30]*e[19]*e[26]-e[24]*e[31]*e[20]+e[24]*e[28]*e[23]+e[27]*e[22]*e[26]+e[30]*e[25]*e[20]-e[33]*e[22]*e[20]+e[33]*e[19]*e[23]+e[21]*e[34]*e[20]-e[18]*e[23]*e[34]-e[24]*e[22]*e[29]-e[18]*e[32]*e[25]+e[18]*e[22]*e[35];
A[155]=e[12]*e[14]*e[8]+e[12]*e[5]*e[17]+e[15]*e[16]*e[7]+e[15]*e[17]*e[8]+e[0]*e[11]*e[17]+e[0]*e[9]*e[15]+e[0]*e[10]*e[16]+e[3]*e[14]*e[17]+e[3]*e[13]*e[16]+e[9]*e[10]*e[7]+e[9]*e[1]*e[16]+e[9]*e[11]*e[8]+e[9]*e[2]*e[17]-1.*e[15]*e[11]*e[2]-1.*e[15]*e[10]*e[1]-1.*e[15]*e[13]*e[4]-1.*e[15]*e[14]*e[5]+e[12]*e[3]*e[15]+e[12]*e[13]*e[7]+e[12]*e[4]*e[16]+.5000000000*ep2[12]*e[6]+1.500000000*ep2[15]*e[6]+.5000000000*e[6]*ep2[17]+.5000000000*e[6]*ep2[16]+.5000000000*e[6]*ep2[9]-.5000000000*e[6]*ep2[11]-.5000000000*e[6]*ep2[10]-.5000000000*e[6]*ep2[14]-.5000000000*e[6]*ep2[13];
A[8]=-e[9]*e[14]*e[16]-e[12]*e[10]*e[17]+e[9]*e[13]*e[17]-e[15]*e[13]*e[11]+e[15]*e[10]*e[14]+e[12]*e[16]*e[11];
A[154]=e[21]*e[14]*e[17]+e[21]*e[13]*e[16]+e[15]*e[26]*e[17]+e[15]*e[25]*e[16]-1.*e[15]*e[23]*e[14]-1.*e[15]*e[20]*e[11]-1.*e[15]*e[19]*e[10]-1.*e[15]*e[22]*e[13]+e[9]*e[20]*e[17]+e[9]*e[11]*e[26]+e[9]*e[19]*e[16]+e[9]*e[10]*e[25]+.5000000000*ep2[12]*e[24]+1.500000000*e[24]*ep2[15]+.5000000000*e[24]*ep2[17]+.5000000000*e[24]*ep2[16]+.5000000000*ep2[9]*e[24]-.5000000000*e[24]*ep2[11]-.5000000000*e[24]*ep2[10]-.5000000000*e[24]*ep2[14]-.5000000000*e[24]*ep2[13]+e[18]*e[11]*e[17]+e[18]*e[9]*e[15]+e[18]*e[10]*e[16]+e[12]*e[21]*e[15]+e[12]*e[23]*e[17]+e[12]*e[14]*e[26]+e[12]*e[22]*e[16]+e[12]*e[13]*e[25];
A[11]=-e[9]*e[5]*e[34]+e[9]*e[31]*e[8]-e[9]*e[32]*e[7]+e[27]*e[4]*e[17]+e[27]*e[13]*e[8]-e[27]*e[5]*e[16]-e[27]*e[14]*e[7]+e[0]*e[13]*e[35]-e[0]*e[32]*e[16]+e[0]*e[31]*e[17]-e[0]*e[14]*e[34]+e[9]*e[4]*e[35]+e[6]*e[10]*e[32]+e[6]*e[28]*e[14]-e[6]*e[13]*e[29]-e[6]*e[31]*e[11]+e[15]*e[1]*e[32]+e[3]*e[34]*e[11]+e[3]*e[16]*e[29]-e[3]*e[10]*e[35]-e[3]*e[28]*e[17]-e[12]*e[1]*e[35]+e[12]*e[7]*e[29]+e[12]*e[34]*e[2]-e[12]*e[28]*e[8]+e[15]*e[28]*e[5]-e[15]*e[4]*e[29]-e[15]*e[31]*e[2]+e[33]*e[1]*e[14]+e[33]*e[10]*e[5]-e[33]*e[4]*e[11]-e[33]*e[13]*e[2]+e[30]*e[7]*e[11]+e[30]*e[16]*e[2]-e[30]*e[1]*e[17]-e[30]*e[10]*e[8];
A[153]=e[21]*e[31]*e[7]+e[21]*e[4]*e[34]+e[21]*e[32]*e[8]+e[21]*e[5]*e[35]+e[30]*e[22]*e[7]+e[30]*e[4]*e[25]+e[30]*e[23]*e[8]+e[30]*e[5]*e[26]+3.*e[24]*e[33]*e[6]+e[24]*e[34]*e[7]+e[24]*e[35]*e[8]+e[33]*e[25]*e[7]+e[33]*e[26]*e[8]+e[0]*e[27]*e[24]+e[0]*e[18]*e[33]+e[0]*e[28]*e[25]+e[0]*e[19]*e[34]+e[0]*e[29]*e[26]+e[0]*e[20]*e[35]+e[18]*e[27]*e[6]+e[18]*e[28]*e[7]+e[18]*e[1]*e[34]+e[18]*e[29]*e[8]+e[18]*e[2]*e[35]+e[27]*e[19]*e[7]+e[27]*e[1]*e[25]+e[27]*e[20]*e[8]+e[27]*e[2]*e[26]+e[3]*e[30]*e[24]+e[3]*e[21]*e[33]+e[3]*e[31]*e[25]+e[3]*e[22]*e[34]+e[3]*e[32]*e[26]+e[3]*e[23]*e[35]+e[6]*e[30]*e[21]-1.*e[6]*e[29]*e[20]+e[6]*e[35]*e[26]-1.*e[6]*e[31]*e[22]-1.*e[6]*e[32]*e[23]-1.*e[6]*e[28]*e[19]+e[6]*e[34]*e[25]-1.*e[24]*e[32]*e[5]-1.*e[24]*e[29]*e[2]-1.*e[24]*e[28]*e[1]-1.*e[24]*e[31]*e[4]-1.*e[33]*e[20]*e[2]-1.*e[33]*e[19]*e[1]-1.*e[33]*e[22]*e[4]-1.*e[33]*e[23]*e[5];
A[10]=e[21]*e[25]*e[20]-e[21]*e[19]*e[26]+e[18]*e[22]*e[26]-e[18]*e[23]*e[25]-e[24]*e[22]*e[20]+e[24]*e[19]*e[23];
A[152]=e[3]*e[4]*e[25]+e[3]*e[23]*e[8]+e[3]*e[5]*e[26]+e[21]*e[4]*e[7]+e[21]*e[5]*e[8]+e[6]*e[25]*e[7]+e[6]*e[26]*e[8]+e[0]*e[19]*e[7]+e[0]*e[1]*e[25]+e[0]*e[20]*e[8]+e[0]*e[2]*e[26]-1.*e[6]*e[20]*e[2]-1.*e[6]*e[19]*e[1]-1.*e[6]*e[22]*e[4]-1.*e[6]*e[23]*e[5]+e[18]*e[1]*e[7]+e[18]*e[0]*e[6]+e[18]*e[2]*e[8]+e[3]*e[21]*e[6]+e[3]*e[22]*e[7]-.5000000000*e[24]*ep2[4]+.5000000000*e[24]*ep2[0]+1.500000000*e[24]*ep2[6]-.5000000000*e[24]*ep2[5]-.5000000000*e[24]*ep2[1]+.5000000000*e[24]*ep2[7]+.5000000000*e[24]*ep2[3]-.5000000000*e[24]*ep2[2]+.5000000000*e[24]*ep2[8];
A[13]=e[6]*e[28]*e[23]-e[6]*e[22]*e[29]-e[6]*e[31]*e[20]-e[3]*e[19]*e[35]+e[3]*e[34]*e[20]+e[3]*e[25]*e[29]-e[21]*e[1]*e[35]+e[21]*e[7]*e[29]+e[21]*e[34]*e[2]+e[24]*e[1]*e[32]+e[24]*e[28]*e[5]-e[24]*e[4]*e[29]-e[24]*e[31]*e[2]+e[33]*e[1]*e[23]+e[33]*e[19]*e[5]-e[33]*e[4]*e[20]-e[33]*e[22]*e[2]-e[21]*e[28]*e[8]+e[30]*e[7]*e[20]+e[30]*e[25]*e[2]-e[30]*e[1]*e[26]+e[18]*e[4]*e[35]-e[18]*e[5]*e[34]+e[18]*e[31]*e[8]-e[18]*e[32]*e[7]+e[27]*e[4]*e[26]+e[27]*e[22]*e[8]-e[27]*e[5]*e[25]-e[27]*e[23]*e[7]-e[3]*e[28]*e[26]-e[0]*e[32]*e[25]+e[0]*e[22]*e[35]-e[0]*e[23]*e[34]+e[0]*e[31]*e[26]-e[30]*e[19]*e[8]+e[6]*e[19]*e[32];
A[159]=.5000000000*ep2[18]*e[6]+.5000000000*ep2[21]*e[6]+1.500000000*ep2[24]*e[6]+.5000000000*e[6]*ep2[26]-.5000000000*e[6]*ep2[23]-.5000000000*e[6]*ep2[19]-.5000000000*e[6]*ep2[20]-.5000000000*e[6]*ep2[22]+.5000000000*e[6]*ep2[25]+e[21]*e[3]*e[24]+e[18]*e[20]*e[8]+e[21]*e[4]*e[25]+e[18]*e[19]*e[7]+e[18]*e[1]*e[25]+e[21]*e[22]*e[7]+e[21]*e[23]*e[8]+e[18]*e[0]*e[24]+e[18]*e[2]*e[26]+e[21]*e[5]*e[26]+e[24]*e[26]*e[8]-1.*e[24]*e[20]*e[2]-1.*e[24]*e[19]*e[1]-1.*e[24]*e[22]*e[4]+e[24]*e[25]*e[7]-1.*e[24]*e[23]*e[5]+e[0]*e[19]*e[25]+e[0]*e[20]*e[26]+e[3]*e[22]*e[25]+e[3]*e[23]*e[26];
A[12]=e[18]*e[4]*e[8]+e[3]*e[7]*e[20]+e[3]*e[25]*e[2]-e[3]*e[1]*e[26]-e[18]*e[5]*e[7]+e[6]*e[1]*e[23]+e[6]*e[19]*e[5]-e[6]*e[4]*e[20]-e[6]*e[22]*e[2]+e[21]*e[7]*e[2]-e[21]*e[1]*e[8]+e[24]*e[1]*e[5]-e[24]*e[4]*e[2]-e[3]*e[19]*e[8]+e[0]*e[4]*e[26]+e[0]*e[22]*e[8]-e[0]*e[5]*e[25]-e[0]*e[23]*e[7];
A[158]=e[9]*e[1]*e[7]+e[9]*e[0]*e[6]+e[9]*e[2]*e[8]+e[3]*e[12]*e[6]+e[3]*e[13]*e[7]+e[3]*e[4]*e[16]+e[3]*e[14]*e[8]+e[3]*e[5]*e[17]+e[12]*e[4]*e[7]+e[12]*e[5]*e[8]+e[6]*e[16]*e[7]+e[6]*e[17]*e[8]-1.*e[6]*e[11]*e[2]-1.*e[6]*e[10]*e[1]-1.*e[6]*e[13]*e[4]-1.*e[6]*e[14]*e[5]+e[0]*e[10]*e[7]+e[0]*e[1]*e[16]+e[0]*e[11]*e[8]+e[0]*e[2]*e[17]+.5000000000*ep2[3]*e[15]+1.500000000*e[15]*ep2[6]+.5000000000*e[15]*ep2[7]+.5000000000*e[15]*ep2[8]+.5000000000*ep2[0]*e[15]-.5000000000*e[15]*ep2[4]-.5000000000*e[15]*ep2[5]-.5000000000*e[15]*ep2[1]-.5000000000*e[15]*ep2[2];
A[15]=-e[15]*e[13]*e[2]-e[6]*e[13]*e[11]-e[15]*e[4]*e[11]+e[12]*e[16]*e[2]-e[3]*e[10]*e[17]+e[3]*e[16]*e[11]+e[0]*e[13]*e[17]-e[0]*e[14]*e[16]+e[15]*e[1]*e[14]-e[12]*e[10]*e[8]+e[9]*e[4]*e[17]+e[9]*e[13]*e[8]-e[9]*e[5]*e[16]-e[9]*e[14]*e[7]+e[15]*e[10]*e[5]+e[12]*e[7]*e[11]+e[6]*e[10]*e[14]-e[12]*e[1]*e[17];
A[157]=e[12]*e[30]*e[24]+e[12]*e[21]*e[33]+e[12]*e[31]*e[25]+e[12]*e[22]*e[34]+e[12]*e[32]*e[26]+e[12]*e[23]*e[35]+e[9]*e[27]*e[24]+e[9]*e[18]*e[33]+e[9]*e[28]*e[25]+e[9]*e[19]*e[34]+e[9]*e[29]*e[26]+e[9]*e[20]*e[35]+e[21]*e[30]*e[15]+e[21]*e[32]*e[17]+e[21]*e[14]*e[35]+e[21]*e[31]*e[16]+e[21]*e[13]*e[34]+e[30]*e[23]*e[17]+e[30]*e[14]*e[26]+e[30]*e[22]*e[16]+e[30]*e[13]*e[25]+e[15]*e[27]*e[18]+3.*e[15]*e[33]*e[24]-1.*e[15]*e[29]*e[20]+e[15]*e[35]*e[26]-1.*e[15]*e[31]*e[22]-1.*e[15]*e[32]*e[23]-1.*e[15]*e[28]*e[19]+e[15]*e[34]*e[25]+e[18]*e[29]*e[17]+e[18]*e[11]*e[35]+e[18]*e[28]*e[16]+e[18]*e[10]*e[34]+e[27]*e[20]*e[17]+e[27]*e[11]*e[26]+e[27]*e[19]*e[16]+e[27]*e[10]*e[25]-1.*e[24]*e[28]*e[10]-1.*e[24]*e[31]*e[13]-1.*e[24]*e[32]*e[14]+e[24]*e[34]*e[16]+e[24]*e[35]*e[17]-1.*e[24]*e[29]*e[11]-1.*e[33]*e[23]*e[14]+e[33]*e[25]*e[16]+e[33]*e[26]*e[17]-1.*e[33]*e[20]*e[11]-1.*e[33]*e[19]*e[10]-1.*e[33]*e[22]*e[13];
A[14]=e[18]*e[13]*e[17]+e[9]*e[13]*e[26]+e[9]*e[22]*e[17]-e[9]*e[14]*e[25]-e[18]*e[14]*e[16]-e[15]*e[13]*e[20]-e[15]*e[22]*e[11]+e[12]*e[16]*e[20]+e[12]*e[25]*e[11]-e[12]*e[10]*e[26]-e[12]*e[19]*e[17]+e[21]*e[16]*e[11]-e[21]*e[10]*e[17]-e[9]*e[23]*e[16]+e[24]*e[10]*e[14]-e[24]*e[13]*e[11]+e[15]*e[10]*e[23]+e[15]*e[19]*e[14];
A[156]=e[21]*e[12]*e[24]+e[21]*e[23]*e[17]+e[21]*e[14]*e[26]+e[21]*e[22]*e[16]+e[21]*e[13]*e[25]+e[24]*e[26]*e[17]+e[24]*e[25]*e[16]+e[9]*e[19]*e[25]+e[9]*e[18]*e[24]+e[9]*e[20]*e[26]+e[12]*e[22]*e[25]+e[12]*e[23]*e[26]+e[18]*e[20]*e[17]+e[18]*e[11]*e[26]+e[18]*e[19]*e[16]+e[18]*e[10]*e[25]-1.*e[24]*e[23]*e[14]-1.*e[24]*e[20]*e[11]-1.*e[24]*e[19]*e[10]-1.*e[24]*e[22]*e[13]+.5000000000*ep2[21]*e[15]+1.500000000*ep2[24]*e[15]+.5000000000*e[15]*ep2[25]+.5000000000*e[15]*ep2[26]+.5000000000*e[15]*ep2[18]-.5000000000*e[15]*ep2[23]-.5000000000*e[15]*ep2[19]-.5000000000*e[15]*ep2[20]-.5000000000*e[15]*ep2[22];
A[18]=e[6]*e[1]*e[14]+e[15]*e[1]*e[5]-e[0]*e[5]*e[16]-e[0]*e[14]*e[7]+e[0]*e[13]*e[8]-e[15]*e[4]*e[2]+e[12]*e[7]*e[2]+e[6]*e[10]*e[5]+e[3]*e[7]*e[11]-e[6]*e[4]*e[11]+e[3]*e[16]*e[2]-e[6]*e[13]*e[2]-e[3]*e[1]*e[17]-e[9]*e[5]*e[7]-e[3]*e[10]*e[8]-e[12]*e[1]*e[8]+e[0]*e[4]*e[17]+e[9]*e[4]*e[8];
A[128]=-.5000000000*e[14]*ep2[16]-.5000000000*e[14]*ep2[10]-.5000000000*e[14]*ep2[9]+e[11]*e[9]*e[12]+.5000000000*ep3[14]+e[17]*e[13]*e[16]+.5000000000*e[14]*ep2[12]+e[11]*e[10]*e[13]-.5000000000*e[14]*ep2[15]+.5000000000*e[14]*ep2[17]+e[17]*e[12]*e[15]+.5000000000*ep2[11]*e[14]+.5000000000*e[14]*ep2[13];
A[19]=-e[21]*e[19]*e[8]+e[18]*e[4]*e[26]-e[18]*e[5]*e[25]-e[18]*e[23]*e[7]+e[21]*e[25]*e[2]-e[21]*e[1]*e[26]+e[6]*e[19]*e[23]+e[18]*e[22]*e[8]-e[0]*e[23]*e[25]-e[6]*e[22]*e[20]+e[24]*e[1]*e[23]+e[24]*e[19]*e[5]-e[24]*e[4]*e[20]-e[24]*e[22]*e[2]+e[3]*e[25]*e[20]-e[3]*e[19]*e[26]+e[0]*e[22]*e[26]+e[21]*e[7]*e[20];
A[129]=.5000000000*ep2[20]*e[32]+1.500000000*e[32]*ep2[23]+.5000000000*e[32]*ep2[22]+.5000000000*e[32]*ep2[21]+.5000000000*e[32]*ep2[26]-.5000000000*e[32]*ep2[18]-.5000000000*e[32]*ep2[19]-.5000000000*e[32]*ep2[24]-.5000000000*e[32]*ep2[25]+e[20]*e[27]*e[21]+e[20]*e[18]*e[30]+e[20]*e[28]*e[22]+e[20]*e[19]*e[31]+e[20]*e[29]*e[23]+e[29]*e[19]*e[22]+e[29]*e[18]*e[21]+e[23]*e[30]*e[21]+e[23]*e[31]*e[22]+e[26]*e[30]*e[24]+e[26]*e[21]*e[33]+e[26]*e[31]*e[25]+e[26]*e[22]*e[34]+e[26]*e[23]*e[35]+e[35]*e[22]*e[25]+e[35]*e[21]*e[24]-1.*e[23]*e[27]*e[18]-1.*e[23]*e[33]*e[24]-1.*e[23]*e[28]*e[19]-1.*e[23]*e[34]*e[25];
A[16]=-e[9]*e[23]*e[25]-e[21]*e[10]*e[26]-e[21]*e[19]*e[17]-e[18]*e[23]*e[16]+e[18]*e[13]*e[26]+e[12]*e[25]*e[20]-e[12]*e[19]*e[26]-e[15]*e[22]*e[20]+e[21]*e[16]*e[20]+e[21]*e[25]*e[11]+e[24]*e[10]*e[23]+e[24]*e[19]*e[14]-e[24]*e[13]*e[20]-e[24]*e[22]*e[11]+e[18]*e[22]*e[17]-e[18]*e[14]*e[25]+e[9]*e[22]*e[26]+e[15]*e[19]*e[23];
A[130]=.5000000000*e[23]*ep2[21]+e[20]*e[19]*e[22]+e[20]*e[18]*e[21]+.5000000000*ep3[23]+e[26]*e[22]*e[25]+.5000000000*e[23]*ep2[26]-.5000000000*e[23]*ep2[18]+.5000000000*e[23]*ep2[22]-.5000000000*e[23]*ep2[19]+e[26]*e[21]*e[24]+.5000000000*ep2[20]*e[23]-.5000000000*e[23]*ep2[24]-.5000000000*e[23]*ep2[25];
A[17]=e[18]*e[13]*e[35]-e[18]*e[32]*e[16]+e[18]*e[31]*e[17]-e[18]*e[14]*e[34]+e[27]*e[13]*e[26]+e[27]*e[22]*e[17]-e[27]*e[14]*e[25]-e[27]*e[23]*e[16]-e[9]*e[32]*e[25]+e[9]*e[22]*e[35]-e[9]*e[23]*e[34]+e[9]*e[31]*e[26]+e[15]*e[19]*e[32]+e[15]*e[28]*e[23]-e[15]*e[22]*e[29]-e[15]*e[31]*e[20]+e[24]*e[10]*e[32]+e[24]*e[28]*e[14]-e[24]*e[13]*e[29]-e[24]*e[31]*e[11]+e[33]*e[10]*e[23]+e[33]*e[19]*e[14]-e[33]*e[13]*e[20]-e[33]*e[22]*e[11]+e[21]*e[16]*e[29]-e[21]*e[10]*e[35]-e[21]*e[28]*e[17]+e[30]*e[16]*e[20]+e[30]*e[25]*e[11]-e[30]*e[10]*e[26]-e[30]*e[19]*e[17]-e[12]*e[28]*e[26]-e[12]*e[19]*e[35]+e[12]*e[34]*e[20]+e[12]*e[25]*e[29]+e[21]*e[34]*e[11];
A[131]=-1.*e[32]*e[10]*e[1]+e[32]*e[13]*e[4]-1.*e[32]*e[16]*e[7]-1.*e[32]*e[15]*e[6]-1.*e[32]*e[9]*e[0]+e[32]*e[12]*e[3]+e[17]*e[30]*e[6]+e[17]*e[3]*e[33]+e[17]*e[31]*e[7]+e[17]*e[4]*e[34]+e[17]*e[5]*e[35]-1.*e[5]*e[27]*e[9]-1.*e[5]*e[28]*e[10]-1.*e[5]*e[33]*e[15]-1.*e[5]*e[34]*e[16]+e[5]*e[29]*e[11]+e[35]*e[12]*e[6]+e[35]*e[3]*e[15]+e[35]*e[13]*e[7]+e[35]*e[4]*e[16]+e[11]*e[27]*e[3]+e[11]*e[0]*e[30]+e[11]*e[28]*e[4]+e[11]*e[1]*e[31]+e[29]*e[9]*e[3]+e[29]*e[0]*e[12]+e[29]*e[10]*e[4]+e[29]*e[1]*e[13]+e[5]*e[30]*e[12]+3.*e[5]*e[32]*e[14]+e[5]*e[31]*e[13]+e[8]*e[30]*e[15]+e[8]*e[12]*e[33]+e[8]*e[32]*e[17]+e[8]*e[14]*e[35]+e[8]*e[31]*e[16]+e[8]*e[13]*e[34]+e[2]*e[27]*e[12]+e[2]*e[9]*e[30]+e[2]*e[29]*e[14]+e[2]*e[11]*e[32]+e[2]*e[28]*e[13]+e[2]*e[10]*e[31]-1.*e[14]*e[27]*e[0]-1.*e[14]*e[34]*e[7]-1.*e[14]*e[33]*e[6]+e[14]*e[30]*e[3]-1.*e[14]*e[28]*e[1]+e[14]*e[31]*e[4];
A[22]=.5000000000*e[18]*ep2[29]+.5000000000*e[18]*ep2[28]+.5000000000*e[18]*ep2[30]+.5000000000*e[18]*ep2[33]-.5000000000*e[18]*ep2[32]-.5000000000*e[18]*ep2[31]-.5000000000*e[18]*ep2[34]-.5000000000*e[18]*ep2[35]+1.500000000*e[18]*ep2[27]+e[27]*e[28]*e[19]+e[27]*e[29]*e[20]+e[21]*e[27]*e[30]+e[21]*e[29]*e[32]+e[21]*e[28]*e[31]+e[30]*e[28]*e[22]+e[30]*e[19]*e[31]+e[30]*e[29]*e[23]+e[30]*e[20]*e[32]+e[24]*e[27]*e[33]+e[24]*e[29]*e[35]+e[24]*e[28]*e[34]+e[33]*e[28]*e[25]+e[33]*e[19]*e[34]+e[33]*e[29]*e[26]+e[33]*e[20]*e[35]-1.*e[27]*e[35]*e[26]-1.*e[27]*e[31]*e[22]-1.*e[27]*e[32]*e[23]-1.*e[27]*e[34]*e[25];
A[132]=e[20]*e[1]*e[4]+e[20]*e[0]*e[3]+e[20]*e[2]*e[5]+e[5]*e[21]*e[3]+e[5]*e[22]*e[4]+e[8]*e[21]*e[6]+e[8]*e[3]*e[24]+e[8]*e[22]*e[7]+e[8]*e[4]*e[25]+e[8]*e[5]*e[26]+e[26]*e[4]*e[7]+e[26]*e[3]*e[6]+e[2]*e[18]*e[3]+e[2]*e[0]*e[21]+e[2]*e[19]*e[4]+e[2]*e[1]*e[22]-1.*e[5]*e[19]*e[1]-1.*e[5]*e[18]*e[0]-1.*e[5]*e[25]*e[7]-1.*e[5]*e[24]*e[6]+.5000000000*e[23]*ep2[4]-.5000000000*e[23]*ep2[0]-.5000000000*e[23]*ep2[6]+1.500000000*e[23]*ep2[5]-.5000000000*e[23]*ep2[1]-.5000000000*e[23]*ep2[7]+.5000000000*e[23]*ep2[3]+.5000000000*e[23]*ep2[2]+.5000000000*e[23]*ep2[8];
A[23]=1.500000000*e[9]*ep2[27]+.5000000000*e[9]*ep2[29]+.5000000000*e[9]*ep2[28]-.5000000000*e[9]*ep2[32]-.5000000000*e[9]*ep2[31]+.5000000000*e[9]*ep2[33]+.5000000000*e[9]*ep2[30]-.5000000000*e[9]*ep2[34]-.5000000000*e[9]*ep2[35]+e[33]*e[27]*e[15]+e[33]*e[29]*e[17]+e[33]*e[11]*e[35]+e[33]*e[28]*e[16]+e[33]*e[10]*e[34]+e[27]*e[29]*e[11]+e[27]*e[28]*e[10]+e[27]*e[30]*e[12]-1.*e[27]*e[31]*e[13]-1.*e[27]*e[32]*e[14]-1.*e[27]*e[34]*e[16]-1.*e[27]*e[35]*e[17]+e[30]*e[29]*e[14]+e[30]*e[11]*e[32]+e[30]*e[28]*e[13]+e[30]*e[10]*e[31]+e[12]*e[29]*e[32]+e[12]*e[28]*e[31]+e[15]*e[29]*e[35]+e[15]*e[28]*e[34];
A[133]=-1.*e[32]*e[24]*e[6]+e[8]*e[30]*e[24]+e[8]*e[21]*e[33]+e[8]*e[31]*e[25]+e[8]*e[22]*e[34]+e[26]*e[30]*e[6]+e[26]*e[3]*e[33]+e[26]*e[31]*e[7]+e[26]*e[4]*e[34]+e[26]*e[32]*e[8]+e[26]*e[5]*e[35]+e[35]*e[21]*e[6]+e[35]*e[3]*e[24]+e[35]*e[22]*e[7]+e[35]*e[4]*e[25]+e[35]*e[23]*e[8]+e[2]*e[27]*e[21]+e[2]*e[18]*e[30]+e[2]*e[28]*e[22]+e[2]*e[19]*e[31]+e[2]*e[29]*e[23]+e[2]*e[20]*e[32]+e[20]*e[27]*e[3]+e[20]*e[0]*e[30]+e[20]*e[28]*e[4]+e[20]*e[1]*e[31]+e[20]*e[29]*e[5]+e[29]*e[18]*e[3]+e[29]*e[0]*e[21]+e[29]*e[19]*e[4]+e[29]*e[1]*e[22]+e[5]*e[30]*e[21]+e[5]*e[31]*e[22]+3.*e[5]*e[32]*e[23]-1.*e[5]*e[27]*e[18]-1.*e[5]*e[33]*e[24]-1.*e[5]*e[28]*e[19]-1.*e[5]*e[34]*e[25]-1.*e[23]*e[27]*e[0]-1.*e[23]*e[34]*e[7]-1.*e[23]*e[33]*e[6]+e[23]*e[30]*e[3]-1.*e[23]*e[28]*e[1]+e[23]*e[31]*e[4]+e[32]*e[21]*e[3]-1.*e[32]*e[19]*e[1]+e[32]*e[22]*e[4]-1.*e[32]*e[18]*e[0]-1.*e[32]*e[25]*e[7];
A[20]=.5000000000*e[27]*ep2[33]-.5000000000*e[27]*ep2[32]-.5000000000*e[27]*ep2[31]-.5000000000*e[27]*ep2[34]-.5000000000*e[27]*ep2[35]+e[33]*e[29]*e[35]+.5000000000*e[27]*ep2[29]+e[30]*e[29]*e[32]+e[30]*e[28]*e[31]+e[33]*e[28]*e[34]+.5000000000*e[27]*ep2[28]+.5000000000*e[27]*ep2[30]+.5000000000*ep3[27];
A[134]=e[14]*e[21]*e[12]+e[14]*e[22]*e[13]+e[17]*e[21]*e[15]+e[17]*e[12]*e[24]+e[17]*e[14]*e[26]+e[17]*e[22]*e[16]+e[17]*e[13]*e[25]+e[26]*e[12]*e[15]+e[26]*e[13]*e[16]-1.*e[14]*e[24]*e[15]-1.*e[14]*e[25]*e[16]-1.*e[14]*e[18]*e[9]-1.*e[14]*e[19]*e[10]+e[11]*e[18]*e[12]+e[11]*e[9]*e[21]+e[11]*e[19]*e[13]+e[11]*e[10]*e[22]+e[20]*e[11]*e[14]+e[20]*e[9]*e[12]+e[20]*e[10]*e[13]+1.500000000*e[23]*ep2[14]+.5000000000*e[23]*ep2[12]+.5000000000*e[23]*ep2[13]+.5000000000*e[23]*ep2[17]+.5000000000*ep2[11]*e[23]-.5000000000*e[23]*ep2[16]-.5000000000*e[23]*ep2[9]-.5000000000*e[23]*ep2[15]-.5000000000*e[23]*ep2[10];
A[21]=1.500000000*e[0]*ep2[27]+.5000000000*e[0]*ep2[29]+.5000000000*e[0]*ep2[28]+.5000000000*e[0]*ep2[30]-.5000000000*e[0]*ep2[32]-.5000000000*e[0]*ep2[31]+.5000000000*e[0]*ep2[33]-.5000000000*e[0]*ep2[34]-.5000000000*e[0]*ep2[35]-1.*e[27]*e[31]*e[4]+e[3]*e[27]*e[30]+e[3]*e[29]*e[32]+e[3]*e[28]*e[31]+e[30]*e[28]*e[4]+e[30]*e[1]*e[31]+e[30]*e[29]*e[5]+e[30]*e[2]*e[32]+e[6]*e[27]*e[33]+e[6]*e[29]*e[35]+e[6]*e[28]*e[34]+e[27]*e[28]*e[1]+e[27]*e[29]*e[2]+e[33]*e[28]*e[7]+e[33]*e[1]*e[34]+e[33]*e[29]*e[8]+e[33]*e[2]*e[35]-1.*e[27]*e[34]*e[7]-1.*e[27]*e[32]*e[5]-1.*e[27]*e[35]*e[8];
A[135]=e[14]*e[12]*e[3]+e[14]*e[13]*e[4]+e[17]*e[12]*e[6]+e[17]*e[3]*e[15]+e[17]*e[13]*e[7]+e[17]*e[4]*e[16]+e[17]*e[14]*e[8]+e[8]*e[12]*e[15]+e[8]*e[13]*e[16]+e[2]*e[11]*e[14]+e[2]*e[9]*e[12]+e[2]*e[10]*e[13]+e[11]*e[9]*e[3]+e[11]*e[0]*e[12]+e[11]*e[10]*e[4]+e[11]*e[1]*e[13]-1.*e[14]*e[10]*e[1]-1.*e[14]*e[16]*e[7]-1.*e[14]*e[15]*e[6]-1.*e[14]*e[9]*e[0]-.5000000000*e[5]*ep2[16]-.5000000000*e[5]*ep2[9]+.5000000000*e[5]*ep2[11]+.5000000000*e[5]*ep2[12]-.5000000000*e[5]*ep2[15]-.5000000000*e[5]*ep2[10]+.5000000000*e[5]*ep2[13]+1.500000000*ep2[14]*e[5]+.5000000000*e[5]*ep2[17];
A[27]=1.500000000*e[27]*ep2[9]-.5000000000*e[27]*ep2[16]+.5000000000*e[27]*ep2[11]+.5000000000*e[27]*ep2[12]+.5000000000*e[27]*ep2[15]-.5000000000*e[27]*ep2[17]+.5000000000*e[27]*ep2[10]-.5000000000*e[27]*ep2[14]-.5000000000*e[27]*ep2[13]+e[12]*e[10]*e[31]+e[30]*e[11]*e[14]+e[30]*e[10]*e[13]+e[15]*e[9]*e[33]+e[15]*e[29]*e[17]+e[15]*e[11]*e[35]+e[15]*e[28]*e[16]+e[15]*e[10]*e[34]+e[33]*e[11]*e[17]+e[33]*e[10]*e[16]-1.*e[9]*e[31]*e[13]-1.*e[9]*e[32]*e[14]-1.*e[9]*e[34]*e[16]-1.*e[9]*e[35]*e[17]+e[9]*e[29]*e[11]+e[9]*e[28]*e[10]+e[12]*e[9]*e[30]+e[12]*e[29]*e[14]+e[12]*e[11]*e[32]+e[12]*e[28]*e[13];
A[137]=e[29]*e[18]*e[12]+e[29]*e[9]*e[21]+e[29]*e[19]*e[13]+e[29]*e[10]*e[22]+e[17]*e[30]*e[24]+e[17]*e[21]*e[33]+e[17]*e[31]*e[25]+e[17]*e[22]*e[34]+e[17]*e[32]*e[26]+e[17]*e[23]*e[35]-1.*e[23]*e[27]*e[9]-1.*e[23]*e[28]*e[10]-1.*e[23]*e[33]*e[15]-1.*e[23]*e[34]*e[16]-1.*e[32]*e[24]*e[15]-1.*e[32]*e[25]*e[16]-1.*e[32]*e[18]*e[9]-1.*e[32]*e[19]*e[10]+e[26]*e[30]*e[15]+e[26]*e[12]*e[33]+e[26]*e[31]*e[16]+e[26]*e[13]*e[34]+e[35]*e[21]*e[15]+e[35]*e[12]*e[24]+e[35]*e[22]*e[16]+e[35]*e[13]*e[25]+e[14]*e[30]*e[21]+e[14]*e[31]*e[22]+3.*e[14]*e[32]*e[23]+e[11]*e[27]*e[21]+e[11]*e[18]*e[30]+e[11]*e[28]*e[22]+e[11]*e[19]*e[31]+e[11]*e[29]*e[23]+e[11]*e[20]*e[32]+e[23]*e[30]*e[12]+e[23]*e[31]*e[13]+e[32]*e[21]*e[12]+e[32]*e[22]*e[13]-1.*e[14]*e[27]*e[18]-1.*e[14]*e[33]*e[24]+e[14]*e[29]*e[20]+e[14]*e[35]*e[26]-1.*e[14]*e[28]*e[19]-1.*e[14]*e[34]*e[25]+e[20]*e[27]*e[12]+e[20]*e[9]*e[30]+e[20]*e[28]*e[13]+e[20]*e[10]*e[31];
A[26]=.5000000000*e[0]*ep2[1]+.5000000000*e[0]*ep2[2]+e[6]*e[2]*e[8]+e[6]*e[1]*e[7]+.5000000000*e[0]*ep2[3]+e[3]*e[1]*e[4]+.5000000000*e[0]*ep2[6]+e[3]*e[2]*e[5]-.5000000000*e[0]*ep2[5]-.5000000000*e[0]*ep2[8]+.5000000000*ep3[0]-.5000000000*e[0]*ep2[7]-.5000000000*e[0]*ep2[4];
A[136]=1.500000000*ep2[23]*e[14]+.5000000000*e[14]*ep2[26]-.5000000000*e[14]*ep2[18]-.5000000000*e[14]*ep2[19]+.5000000000*e[14]*ep2[20]+.5000000000*e[14]*ep2[22]-.5000000000*e[14]*ep2[24]+.5000000000*e[14]*ep2[21]-.5000000000*e[14]*ep2[25]+e[23]*e[21]*e[12]+e[23]*e[22]*e[13]+e[26]*e[21]*e[15]+e[26]*e[12]*e[24]+e[26]*e[23]*e[17]+e[26]*e[22]*e[16]+e[26]*e[13]*e[25]+e[17]*e[22]*e[25]+e[17]*e[21]*e[24]+e[11]*e[19]*e[22]+e[11]*e[18]*e[21]+e[11]*e[20]*e[23]+e[20]*e[18]*e[12]+e[20]*e[9]*e[21]+e[20]*e[19]*e[13]+e[20]*e[10]*e[22]-1.*e[23]*e[24]*e[15]-1.*e[23]*e[25]*e[16]-1.*e[23]*e[18]*e[9]-1.*e[23]*e[19]*e[10];
A[25]=1.500000000*e[27]*ep2[0]-.5000000000*e[27]*ep2[4]+.5000000000*e[27]*ep2[6]-.5000000000*e[27]*ep2[5]+.5000000000*e[27]*ep2[1]-.5000000000*e[27]*ep2[7]+.5000000000*e[27]*ep2[3]+.5000000000*e[27]*ep2[2]-.5000000000*e[27]*ep2[8]+e[0]*e[33]*e[6]+e[0]*e[30]*e[3]-1.*e[0]*e[35]*e[8]-1.*e[0]*e[31]*e[4]+e[3]*e[28]*e[4]+e[3]*e[1]*e[31]+e[3]*e[29]*e[5]+e[3]*e[2]*e[32]+e[30]*e[1]*e[4]+e[30]*e[2]*e[5]+e[6]*e[28]*e[7]+e[6]*e[1]*e[34]+e[6]*e[29]*e[8]+e[6]*e[2]*e[35]+e[33]*e[1]*e[7]+e[33]*e[2]*e[8]+e[0]*e[28]*e[1]+e[0]*e[29]*e[2]-1.*e[0]*e[34]*e[7]-1.*e[0]*e[32]*e[5];
A[139]=e[8]*e[22]*e[25]+e[8]*e[21]*e[24]+e[20]*e[18]*e[3]+e[20]*e[0]*e[21]+e[20]*e[19]*e[4]+e[20]*e[1]*e[22]+e[20]*e[2]*e[23]+e[23]*e[21]*e[3]+e[23]*e[22]*e[4]+e[23]*e[26]*e[8]-1.*e[23]*e[19]*e[1]-1.*e[23]*e[18]*e[0]-1.*e[23]*e[25]*e[7]-1.*e[23]*e[24]*e[6]+e[2]*e[19]*e[22]+e[2]*e[18]*e[21]+e[26]*e[21]*e[6]+e[26]*e[3]*e[24]+e[26]*e[22]*e[7]+e[26]*e[4]*e[25]+.5000000000*ep2[20]*e[5]+1.500000000*ep2[23]*e[5]+.5000000000*e[5]*ep2[22]+.5000000000*e[5]*ep2[21]+.5000000000*e[5]*ep2[26]-.5000000000*e[5]*ep2[18]-.5000000000*e[5]*ep2[19]-.5000000000*e[5]*ep2[24]-.5000000000*e[5]*ep2[25];
A[24]=e[24]*e[11]*e[8]+e[24]*e[2]*e[17]+3.*e[9]*e[18]*e[0]+e[9]*e[19]*e[1]+e[9]*e[20]*e[2]+e[18]*e[10]*e[1]+e[18]*e[11]*e[2]+e[3]*e[18]*e[12]+e[3]*e[9]*e[21]+e[3]*e[20]*e[14]+e[3]*e[11]*e[23]+e[3]*e[19]*e[13]+e[3]*e[10]*e[22]+e[6]*e[18]*e[15]+e[6]*e[9]*e[24]+e[6]*e[20]*e[17]+e[6]*e[11]*e[26]+e[6]*e[19]*e[16]+e[6]*e[10]*e[25]+e[0]*e[20]*e[11]+e[0]*e[19]*e[10]-1.*e[9]*e[26]*e[8]-1.*e[9]*e[22]*e[4]-1.*e[9]*e[25]*e[7]-1.*e[9]*e[23]*e[5]+e[12]*e[0]*e[21]+e[12]*e[19]*e[4]+e[12]*e[1]*e[22]+e[12]*e[20]*e[5]+e[12]*e[2]*e[23]-1.*e[18]*e[13]*e[4]-1.*e[18]*e[16]*e[7]-1.*e[18]*e[14]*e[5]-1.*e[18]*e[17]*e[8]+e[21]*e[10]*e[4]+e[21]*e[1]*e[13]+e[21]*e[11]*e[5]+e[21]*e[2]*e[14]+e[15]*e[0]*e[24]+e[15]*e[19]*e[7]+e[15]*e[1]*e[25]+e[15]*e[20]*e[8]+e[15]*e[2]*e[26]-1.*e[0]*e[23]*e[14]-1.*e[0]*e[25]*e[16]-1.*e[0]*e[26]*e[17]-1.*e[0]*e[22]*e[13]+e[24]*e[10]*e[7]+e[24]*e[1]*e[16];
A[138]=e[11]*e[1]*e[4]+e[11]*e[0]*e[3]+e[11]*e[2]*e[5]+e[5]*e[12]*e[3]+e[5]*e[13]*e[4]+e[8]*e[12]*e[6]+e[8]*e[3]*e[15]+e[8]*e[13]*e[7]+e[8]*e[4]*e[16]+e[8]*e[5]*e[17]+e[17]*e[4]*e[7]+e[17]*e[3]*e[6]-1.*e[5]*e[10]*e[1]-1.*e[5]*e[16]*e[7]-1.*e[5]*e[15]*e[6]-1.*e[5]*e[9]*e[0]+e[2]*e[9]*e[3]+e[2]*e[0]*e[12]+e[2]*e[10]*e[4]+e[2]*e[1]*e[13]+.5000000000*ep2[2]*e[14]-.5000000000*e[14]*ep2[0]-.5000000000*e[14]*ep2[6]-.5000000000*e[14]*ep2[1]-.5000000000*e[14]*ep2[7]+1.500000000*e[14]*ep2[5]+.5000000000*e[14]*ep2[4]+.5000000000*e[14]*ep2[3]+.5000000000*e[14]*ep2[8];
A[31]=e[3]*e[27]*e[12]+e[3]*e[9]*e[30]+e[3]*e[29]*e[14]+e[3]*e[11]*e[32]+e[3]*e[28]*e[13]+e[3]*e[10]*e[31]+e[6]*e[27]*e[15]+e[6]*e[9]*e[33]+e[6]*e[29]*e[17]+e[6]*e[11]*e[35]+e[6]*e[28]*e[16]+e[6]*e[10]*e[34]+3.*e[0]*e[27]*e[9]+e[0]*e[29]*e[11]+e[0]*e[28]*e[10]-1.*e[9]*e[34]*e[7]-1.*e[9]*e[32]*e[5]-1.*e[9]*e[35]*e[8]+e[9]*e[29]*e[2]+e[9]*e[28]*e[1]-1.*e[9]*e[31]*e[4]+e[12]*e[0]*e[30]+e[12]*e[28]*e[4]+e[12]*e[1]*e[31]+e[12]*e[29]*e[5]+e[12]*e[2]*e[32]+e[27]*e[11]*e[2]+e[27]*e[10]*e[1]-1.*e[27]*e[13]*e[4]-1.*e[27]*e[16]*e[7]-1.*e[27]*e[14]*e[5]-1.*e[27]*e[17]*e[8]+e[30]*e[10]*e[4]+e[30]*e[1]*e[13]+e[30]*e[11]*e[5]+e[30]*e[2]*e[14]+e[15]*e[0]*e[33]+e[15]*e[28]*e[7]+e[15]*e[1]*e[34]+e[15]*e[29]*e[8]+e[15]*e[2]*e[35]-1.*e[0]*e[31]*e[13]-1.*e[0]*e[32]*e[14]-1.*e[0]*e[34]*e[16]-1.*e[0]*e[35]*e[17]+e[33]*e[10]*e[7]+e[33]*e[1]*e[16]+e[33]*e[11]*e[8]+e[33]*e[2]*e[17];
A[141]=.5000000000*ep2[30]*e[6]+.5000000000*e[6]*ep2[27]-.5000000000*e[6]*ep2[32]-.5000000000*e[6]*ep2[28]-.5000000000*e[6]*ep2[29]-.5000000000*e[6]*ep2[31]+1.500000000*e[6]*ep2[33]+.5000000000*e[6]*ep2[34]+.5000000000*e[6]*ep2[35]+e[0]*e[27]*e[33]+e[0]*e[29]*e[35]+e[0]*e[28]*e[34]+e[3]*e[30]*e[33]+e[3]*e[32]*e[35]+e[3]*e[31]*e[34]+e[30]*e[31]*e[7]+e[30]*e[4]*e[34]+e[30]*e[32]*e[8]+e[30]*e[5]*e[35]+e[27]*e[28]*e[7]+e[27]*e[1]*e[34]+e[27]*e[29]*e[8]+e[27]*e[2]*e[35]+e[33]*e[34]*e[7]+e[33]*e[35]*e[8]-1.*e[33]*e[32]*e[5]-1.*e[33]*e[29]*e[2]-1.*e[33]*e[28]*e[1]-1.*e[33]*e[31]*e[4];
A[30]=e[24]*e[20]*e[26]+e[21]*e[19]*e[22]-.5000000000*e[18]*ep2[22]-.5000000000*e[18]*ep2[25]+.5000000000*ep3[18]+.5000000000*e[18]*ep2[21]+e[21]*e[20]*e[23]+.5000000000*e[18]*ep2[20]+.5000000000*e[18]*ep2[19]+.5000000000*e[18]*ep2[24]+e[24]*e[19]*e[25]-.5000000000*e[18]*ep2[23]-.5000000000*e[18]*ep2[26];
A[140]=.5000000000*e[33]*ep2[35]+.5000000000*ep3[33]+.5000000000*ep2[27]*e[33]+.5000000000*ep2[30]*e[33]-.5000000000*e[33]*ep2[29]+.5000000000*e[33]*ep2[34]-.5000000000*e[33]*ep2[32]-.5000000000*e[33]*ep2[28]+e[30]*e[32]*e[35]-.5000000000*e[33]*ep2[31]+e[27]*e[29]*e[35]+e[27]*e[28]*e[34]+e[30]*e[31]*e[34];
A[29]=1.500000000*e[27]*ep2[18]+.5000000000*e[27]*ep2[19]+.5000000000*e[27]*ep2[20]+.5000000000*e[27]*ep2[21]+.5000000000*e[27]*ep2[24]-.5000000000*e[27]*ep2[26]-.5000000000*e[27]*ep2[23]-.5000000000*e[27]*ep2[22]-.5000000000*e[27]*ep2[25]+e[33]*e[20]*e[26]-1.*e[18]*e[35]*e[26]-1.*e[18]*e[31]*e[22]-1.*e[18]*e[32]*e[23]-1.*e[18]*e[34]*e[25]+e[18]*e[28]*e[19]+e[18]*e[29]*e[20]+e[21]*e[18]*e[30]+e[21]*e[28]*e[22]+e[21]*e[19]*e[31]+e[21]*e[29]*e[23]+e[21]*e[20]*e[32]+e[30]*e[19]*e[22]+e[30]*e[20]*e[23]+e[24]*e[18]*e[33]+e[24]*e[28]*e[25]+e[24]*e[19]*e[34]+e[24]*e[29]*e[26]+e[24]*e[20]*e[35]+e[33]*e[19]*e[25];
A[143]=e[9]*e[27]*e[33]+e[9]*e[29]*e[35]+e[9]*e[28]*e[34]+e[33]*e[35]*e[17]+e[33]*e[34]*e[16]+e[27]*e[29]*e[17]+e[27]*e[11]*e[35]+e[27]*e[28]*e[16]+e[27]*e[10]*e[34]+e[33]*e[30]*e[12]-1.*e[33]*e[28]*e[10]-1.*e[33]*e[31]*e[13]-1.*e[33]*e[32]*e[14]-1.*e[33]*e[29]*e[11]+e[30]*e[32]*e[17]+e[30]*e[14]*e[35]+e[30]*e[31]*e[16]+e[30]*e[13]*e[34]+e[12]*e[32]*e[35]+e[12]*e[31]*e[34]+.5000000000*e[15]*ep2[27]-.5000000000*e[15]*ep2[32]-.5000000000*e[15]*ep2[28]-.5000000000*e[15]*ep2[29]-.5000000000*e[15]*ep2[31]+1.500000000*e[15]*ep2[33]+.5000000000*e[15]*ep2[30]+.5000000000*e[15]*ep2[34]+.5000000000*e[15]*ep2[35];
A[28]=.5000000000*e[9]*ep2[12]-.5000000000*e[9]*ep2[16]+.5000000000*e[9]*ep2[10]-.5000000000*e[9]*ep2[17]-.5000000000*e[9]*ep2[13]+e[15]*e[10]*e[16]+e[12]*e[11]*e[14]+.5000000000*e[9]*ep2[11]+.5000000000*e[9]*ep2[15]-.5000000000*e[9]*ep2[14]+e[15]*e[11]*e[17]+.5000000000*ep3[9]+e[12]*e[10]*e[13];
A[142]=e[18]*e[27]*e[33]+e[18]*e[29]*e[35]+e[18]*e[28]*e[34]+e[27]*e[28]*e[25]+e[27]*e[19]*e[34]+e[27]*e[29]*e[26]+e[27]*e[20]*e[35]+e[21]*e[30]*e[33]+e[21]*e[32]*e[35]+e[21]*e[31]*e[34]+e[30]*e[31]*e[25]+e[30]*e[22]*e[34]+e[30]*e[32]*e[26]+e[30]*e[23]*e[35]+e[33]*e[34]*e[25]+e[33]*e[35]*e[26]-1.*e[33]*e[29]*e[20]-1.*e[33]*e[31]*e[22]-1.*e[33]*e[32]*e[23]-1.*e[33]*e[28]*e[19]+.5000000000*ep2[27]*e[24]+.5000000000*ep2[30]*e[24]+1.500000000*e[24]*ep2[33]+.5000000000*e[24]*ep2[35]+.5000000000*e[24]*ep2[34]-.5000000000*e[24]*ep2[32]-.5000000000*e[24]*ep2[28]-.5000000000*e[24]*ep2[29]-.5000000000*e[24]*ep2[31];
A[36]=.5000000000*e[9]*ep2[21]+.5000000000*e[9]*ep2[24]+.5000000000*e[9]*ep2[19]+1.500000000*e[9]*ep2[18]+.5000000000*e[9]*ep2[20]-.5000000000*e[9]*ep2[26]-.5000000000*e[9]*ep2[23]-.5000000000*e[9]*ep2[22]-.5000000000*e[9]*ep2[25]+e[21]*e[18]*e[12]+e[21]*e[20]*e[14]+e[21]*e[11]*e[23]+e[21]*e[19]*e[13]+e[21]*e[10]*e[22]+e[24]*e[18]*e[15]+e[24]*e[20]*e[17]+e[24]*e[11]*e[26]+e[24]*e[19]*e[16]+e[24]*e[10]*e[25]+e[15]*e[19]*e[25]+e[15]*e[20]*e[26]+e[12]*e[19]*e[22]+e[12]*e[20]*e[23]+e[18]*e[20]*e[11]+e[18]*e[19]*e[10]-1.*e[18]*e[23]*e[14]-1.*e[18]*e[25]*e[16]-1.*e[18]*e[26]*e[17]-1.*e[18]*e[22]*e[13];
A[182]=.5000000000*ep2[29]*e[26]+.5000000000*ep2[32]*e[26]+.5000000000*e[26]*ep2[33]+1.500000000*e[26]*ep2[35]+.5000000000*e[26]*ep2[34]-.5000000000*e[26]*ep2[27]-.5000000000*e[26]*ep2[28]-.5000000000*e[26]*ep2[31]-.5000000000*e[26]*ep2[30]+e[20]*e[27]*e[33]+e[20]*e[29]*e[35]+e[20]*e[28]*e[34]+e[29]*e[27]*e[24]+e[29]*e[18]*e[33]+e[29]*e[28]*e[25]+e[29]*e[19]*e[34]+e[23]*e[30]*e[33]+e[23]*e[32]*e[35]+e[23]*e[31]*e[34]+e[32]*e[30]*e[24]+e[32]*e[21]*e[33]+e[32]*e[31]*e[25]+e[32]*e[22]*e[34]+e[35]*e[33]*e[24]+e[35]*e[34]*e[25]-1.*e[35]*e[27]*e[18]-1.*e[35]*e[30]*e[21]-1.*e[35]*e[31]*e[22]-1.*e[35]*e[28]*e[19];
A[37]=e[12]*e[19]*e[31]+e[12]*e[29]*e[23]+e[12]*e[20]*e[32]+3.*e[9]*e[27]*e[18]+e[9]*e[28]*e[19]+e[9]*e[29]*e[20]+e[21]*e[9]*e[30]+e[21]*e[29]*e[14]+e[21]*e[11]*e[32]+e[21]*e[28]*e[13]+e[21]*e[10]*e[31]+e[30]*e[20]*e[14]+e[30]*e[11]*e[23]+e[30]*e[19]*e[13]+e[30]*e[10]*e[22]+e[9]*e[33]*e[24]-1.*e[9]*e[35]*e[26]-1.*e[9]*e[31]*e[22]-1.*e[9]*e[32]*e[23]-1.*e[9]*e[34]*e[25]+e[18]*e[29]*e[11]+e[18]*e[28]*e[10]+e[27]*e[20]*e[11]+e[27]*e[19]*e[10]+e[15]*e[27]*e[24]+e[15]*e[18]*e[33]+e[15]*e[28]*e[25]+e[15]*e[19]*e[34]+e[15]*e[29]*e[26]+e[15]*e[20]*e[35]-1.*e[18]*e[31]*e[13]-1.*e[18]*e[32]*e[14]-1.*e[18]*e[34]*e[16]-1.*e[18]*e[35]*e[17]-1.*e[27]*e[23]*e[14]-1.*e[27]*e[25]*e[16]-1.*e[27]*e[26]*e[17]-1.*e[27]*e[22]*e[13]+e[24]*e[29]*e[17]+e[24]*e[11]*e[35]+e[24]*e[28]*e[16]+e[24]*e[10]*e[34]+e[33]*e[20]*e[17]+e[33]*e[11]*e[26]+e[33]*e[19]*e[16]+e[33]*e[10]*e[25]+e[12]*e[27]*e[21]+e[12]*e[18]*e[30]+e[12]*e[28]*e[22];
A[183]=-.5000000000*e[17]*ep2[27]+.5000000000*e[17]*ep2[32]-.5000000000*e[17]*ep2[28]+.5000000000*e[17]*ep2[29]-.5000000000*e[17]*ep2[31]+.5000000000*e[17]*ep2[33]-.5000000000*e[17]*ep2[30]+.5000000000*e[17]*ep2[34]+1.500000000*e[17]*ep2[35]+e[32]*e[30]*e[15]+e[32]*e[12]*e[33]+e[32]*e[31]*e[16]+e[32]*e[13]*e[34]+e[14]*e[30]*e[33]+e[14]*e[31]*e[34]+e[11]*e[27]*e[33]+e[11]*e[29]*e[35]+e[11]*e[28]*e[34]+e[35]*e[33]*e[15]+e[35]*e[34]*e[16]+e[29]*e[27]*e[15]+e[29]*e[9]*e[33]+e[29]*e[28]*e[16]+e[29]*e[10]*e[34]-1.*e[35]*e[27]*e[9]-1.*e[35]*e[30]*e[12]-1.*e[35]*e[28]*e[10]-1.*e[35]*e[31]*e[13]+e[35]*e[32]*e[14];
A[38]=.5000000000*e[9]*ep2[1]+1.500000000*e[9]*ep2[0]+.5000000000*e[9]*ep2[2]+.5000000000*e[9]*ep2[3]+.5000000000*e[9]*ep2[6]-.5000000000*e[9]*ep2[4]-.5000000000*e[9]*ep2[5]-.5000000000*e[9]*ep2[7]-.5000000000*e[9]*ep2[8]+e[6]*e[0]*e[15]+e[6]*e[10]*e[7]+e[6]*e[1]*e[16]+e[6]*e[11]*e[8]+e[6]*e[2]*e[17]+e[15]*e[1]*e[7]+e[15]*e[2]*e[8]+e[0]*e[11]*e[2]+e[0]*e[10]*e[1]-1.*e[0]*e[13]*e[4]-1.*e[0]*e[16]*e[7]-1.*e[0]*e[14]*e[5]-1.*e[0]*e[17]*e[8]+e[3]*e[0]*e[12]+e[3]*e[10]*e[4]+e[3]*e[1]*e[13]+e[3]*e[11]*e[5]+e[3]*e[2]*e[14]+e[12]*e[1]*e[4]+e[12]*e[2]*e[5];
A[180]=.5000000000*e[35]*ep2[33]+.5000000000*e[35]*ep2[34]-.5000000000*e[35]*ep2[27]-.5000000000*e[35]*ep2[28]-.5000000000*e[35]*ep2[31]-.5000000000*e[35]*ep2[30]+e[32]*e[31]*e[34]+.5000000000*ep2[29]*e[35]+.5000000000*ep2[32]*e[35]+e[29]*e[28]*e[34]+e[32]*e[30]*e[33]+.5000000000*ep3[35]+e[29]*e[27]*e[33];
A[39]=.5000000000*e[0]*ep2[19]+.5000000000*e[0]*ep2[20]+.5000000000*e[0]*ep2[24]-.5000000000*e[0]*ep2[26]-.5000000000*e[0]*ep2[23]-.5000000000*e[0]*ep2[22]-.5000000000*e[0]*ep2[25]+1.500000000*ep2[18]*e[0]+.5000000000*e[0]*ep2[21]+e[18]*e[19]*e[1]+e[18]*e[20]*e[2]+e[21]*e[18]*e[3]+e[21]*e[19]*e[4]+e[21]*e[1]*e[22]+e[21]*e[20]*e[5]+e[21]*e[2]*e[23]-1.*e[18]*e[26]*e[8]-1.*e[18]*e[22]*e[4]-1.*e[18]*e[25]*e[7]-1.*e[18]*e[23]*e[5]+e[18]*e[24]*e[6]+e[3]*e[19]*e[22]+e[3]*e[20]*e[23]+e[24]*e[19]*e[7]+e[24]*e[1]*e[25]+e[24]*e[20]*e[8]+e[24]*e[2]*e[26]+e[6]*e[19]*e[25]+e[6]*e[20]*e[26];
A[181]=.5000000000*ep2[32]*e[8]-.5000000000*e[8]*ep2[27]-.5000000000*e[8]*ep2[28]+.5000000000*e[8]*ep2[29]-.5000000000*e[8]*ep2[31]+.5000000000*e[8]*ep2[33]-.5000000000*e[8]*ep2[30]+.5000000000*e[8]*ep2[34]+1.500000000*e[8]*ep2[35]+e[2]*e[27]*e[33]+e[2]*e[29]*e[35]+e[2]*e[28]*e[34]+e[5]*e[30]*e[33]+e[5]*e[32]*e[35]+e[5]*e[31]*e[34]+e[32]*e[30]*e[6]+e[32]*e[3]*e[33]+e[32]*e[31]*e[7]+e[32]*e[4]*e[34]+e[29]*e[27]*e[6]+e[29]*e[0]*e[33]+e[29]*e[28]*e[7]+e[29]*e[1]*e[34]+e[35]*e[33]*e[6]+e[35]*e[34]*e[7]-1.*e[35]*e[27]*e[0]-1.*e[35]*e[30]*e[3]-1.*e[35]*e[28]*e[1]-1.*e[35]*e[31]*e[4];
A[32]=-.5000000000*e[18]*ep2[4]+1.500000000*e[18]*ep2[0]+.5000000000*e[18]*ep2[6]-.5000000000*e[18]*ep2[5]+.5000000000*e[18]*ep2[1]-.5000000000*e[18]*ep2[7]+.5000000000*e[18]*ep2[3]+.5000000000*e[18]*ep2[2]-.5000000000*e[18]*ep2[8]+e[3]*e[0]*e[21]+e[3]*e[19]*e[4]+e[3]*e[1]*e[22]+e[3]*e[20]*e[5]+e[3]*e[2]*e[23]+e[21]*e[1]*e[4]+e[21]*e[2]*e[5]+e[6]*e[0]*e[24]+e[6]*e[19]*e[7]+e[6]*e[1]*e[25]+e[6]*e[20]*e[8]+e[6]*e[2]*e[26]+e[24]*e[1]*e[7]+e[24]*e[2]*e[8]+e[0]*e[19]*e[1]+e[0]*e[20]*e[2]-1.*e[0]*e[26]*e[8]-1.*e[0]*e[22]*e[4]-1.*e[0]*e[25]*e[7]-1.*e[0]*e[23]*e[5];
A[178]=e[10]*e[1]*e[7]+e[10]*e[0]*e[6]+e[10]*e[2]*e[8]+e[4]*e[12]*e[6]+e[4]*e[3]*e[15]+e[4]*e[13]*e[7]+e[4]*e[14]*e[8]+e[4]*e[5]*e[17]+e[13]*e[3]*e[6]+e[13]*e[5]*e[8]+e[7]*e[15]*e[6]+e[7]*e[17]*e[8]-1.*e[7]*e[11]*e[2]-1.*e[7]*e[9]*e[0]-1.*e[7]*e[14]*e[5]-1.*e[7]*e[12]*e[3]+e[1]*e[9]*e[6]+e[1]*e[0]*e[15]+e[1]*e[11]*e[8]+e[1]*e[2]*e[17]+1.500000000*e[16]*ep2[7]+.5000000000*e[16]*ep2[6]+.5000000000*e[16]*ep2[8]+.5000000000*ep2[1]*e[16]-.5000000000*e[16]*ep2[0]-.5000000000*e[16]*ep2[5]-.5000000000*e[16]*ep2[3]-.5000000000*e[16]*ep2[2]+.5000000000*ep2[4]*e[16];
A[33]=e[0]*e[30]*e[21]-1.*e[0]*e[35]*e[26]-1.*e[0]*e[31]*e[22]-1.*e[0]*e[32]*e[23]-1.*e[0]*e[34]*e[25]-1.*e[18]*e[34]*e[7]-1.*e[18]*e[32]*e[5]-1.*e[18]*e[35]*e[8]-1.*e[18]*e[31]*e[4]-1.*e[27]*e[26]*e[8]-1.*e[27]*e[22]*e[4]-1.*e[27]*e[25]*e[7]-1.*e[27]*e[23]*e[5]+e[6]*e[28]*e[25]+e[6]*e[19]*e[34]+e[6]*e[29]*e[26]+e[6]*e[20]*e[35]+e[21]*e[28]*e[4]+e[21]*e[1]*e[31]+e[21]*e[29]*e[5]+e[21]*e[2]*e[32]+e[30]*e[19]*e[4]+e[30]*e[1]*e[22]+e[30]*e[20]*e[5]+e[30]*e[2]*e[23]+e[24]*e[27]*e[6]+e[24]*e[0]*e[33]+e[24]*e[28]*e[7]+e[24]*e[1]*e[34]+e[24]*e[29]*e[8]+e[24]*e[2]*e[35]+e[33]*e[18]*e[6]+e[33]*e[19]*e[7]+e[33]*e[1]*e[25]+e[33]*e[20]*e[8]+e[33]*e[2]*e[26]+3.*e[0]*e[27]*e[18]+e[0]*e[28]*e[19]+e[0]*e[29]*e[20]+e[18]*e[28]*e[1]+e[18]*e[29]*e[2]+e[27]*e[19]*e[1]+e[27]*e[20]*e[2]+e[3]*e[27]*e[21]+e[3]*e[18]*e[30]+e[3]*e[28]*e[22]+e[3]*e[19]*e[31]+e[3]*e[29]*e[23]+e[3]*e[20]*e[32];
A[179]=e[19]*e[18]*e[6]+e[19]*e[0]*e[24]+e[19]*e[1]*e[25]+e[19]*e[20]*e[8]+e[19]*e[2]*e[26]+e[22]*e[21]*e[6]+e[22]*e[3]*e[24]+e[22]*e[4]*e[25]+e[22]*e[23]*e[8]+e[22]*e[5]*e[26]-1.*e[25]*e[21]*e[3]+e[25]*e[26]*e[8]-1.*e[25]*e[20]*e[2]-1.*e[25]*e[18]*e[0]-1.*e[25]*e[23]*e[5]+e[25]*e[24]*e[6]+e[1]*e[18]*e[24]+e[1]*e[20]*e[26]+e[4]*e[21]*e[24]+e[4]*e[23]*e[26]+.5000000000*ep2[19]*e[7]+.5000000000*ep2[22]*e[7]+1.500000000*ep2[25]*e[7]+.5000000000*e[7]*ep2[26]-.5000000000*e[7]*ep2[18]-.5000000000*e[7]*ep2[23]-.5000000000*e[7]*ep2[20]+.5000000000*e[7]*ep2[24]-.5000000000*e[7]*ep2[21];
A[34]=.5000000000*e[18]*ep2[11]+1.500000000*e[18]*ep2[9]+.5000000000*e[18]*ep2[10]+.5000000000*e[18]*ep2[12]+.5000000000*e[18]*ep2[15]-.5000000000*e[18]*ep2[16]-.5000000000*e[18]*ep2[17]-.5000000000*e[18]*ep2[14]-.5000000000*e[18]*ep2[13]+e[12]*e[9]*e[21]+e[12]*e[20]*e[14]+e[12]*e[11]*e[23]+e[12]*e[19]*e[13]+e[12]*e[10]*e[22]+e[21]*e[11]*e[14]+e[21]*e[10]*e[13]+e[15]*e[9]*e[24]+e[15]*e[20]*e[17]+e[15]*e[11]*e[26]+e[15]*e[19]*e[16]+e[15]*e[10]*e[25]+e[24]*e[11]*e[17]+e[24]*e[10]*e[16]-1.*e[9]*e[23]*e[14]-1.*e[9]*e[25]*e[16]-1.*e[9]*e[26]*e[17]+e[9]*e[20]*e[11]+e[9]*e[19]*e[10]-1.*e[9]*e[22]*e[13];
A[176]=e[13]*e[21]*e[24]+e[13]*e[23]*e[26]+e[19]*e[18]*e[15]+e[19]*e[9]*e[24]+e[19]*e[20]*e[17]+e[19]*e[11]*e[26]-1.*e[25]*e[23]*e[14]-1.*e[25]*e[20]*e[11]-1.*e[25]*e[18]*e[9]-1.*e[25]*e[21]*e[12]+e[22]*e[21]*e[15]+e[22]*e[12]*e[24]+e[22]*e[23]*e[17]+e[22]*e[14]*e[26]+e[22]*e[13]*e[25]+e[25]*e[24]*e[15]+e[25]*e[26]*e[17]+e[10]*e[19]*e[25]+e[10]*e[18]*e[24]+e[10]*e[20]*e[26]-.5000000000*e[16]*ep2[18]-.5000000000*e[16]*ep2[23]+.5000000000*e[16]*ep2[19]-.5000000000*e[16]*ep2[20]-.5000000000*e[16]*ep2[21]+.5000000000*ep2[22]*e[16]+1.500000000*ep2[25]*e[16]+.5000000000*e[16]*ep2[24]+.5000000000*e[16]*ep2[26];
A[35]=.5000000000*e[0]*ep2[12]+.5000000000*e[0]*ep2[15]+.5000000000*e[0]*ep2[11]+1.500000000*e[0]*ep2[9]+.5000000000*e[0]*ep2[10]-.5000000000*e[0]*ep2[16]-.5000000000*e[0]*ep2[17]-.5000000000*e[0]*ep2[14]-.5000000000*e[0]*ep2[13]+e[12]*e[9]*e[3]+e[12]*e[10]*e[4]+e[12]*e[1]*e[13]+e[12]*e[11]*e[5]+e[12]*e[2]*e[14]+e[15]*e[9]*e[6]+e[15]*e[10]*e[7]+e[15]*e[1]*e[16]+e[15]*e[11]*e[8]+e[15]*e[2]*e[17]+e[6]*e[11]*e[17]+e[6]*e[10]*e[16]+e[3]*e[11]*e[14]+e[3]*e[10]*e[13]+e[9]*e[10]*e[1]+e[9]*e[11]*e[2]-1.*e[9]*e[13]*e[4]-1.*e[9]*e[16]*e[7]-1.*e[9]*e[14]*e[5]-1.*e[9]*e[17]*e[8];
A[177]=e[19]*e[11]*e[35]+e[28]*e[18]*e[15]+e[28]*e[9]*e[24]+e[28]*e[20]*e[17]+e[28]*e[11]*e[26]-1.*e[25]*e[27]*e[9]-1.*e[25]*e[30]*e[12]-1.*e[25]*e[32]*e[14]+e[25]*e[33]*e[15]+e[25]*e[35]*e[17]-1.*e[25]*e[29]*e[11]-1.*e[34]*e[23]*e[14]+e[34]*e[24]*e[15]+e[34]*e[26]*e[17]-1.*e[34]*e[20]*e[11]-1.*e[34]*e[18]*e[9]-1.*e[34]*e[21]*e[12]+e[13]*e[30]*e[24]+e[13]*e[21]*e[33]+e[13]*e[31]*e[25]+e[13]*e[22]*e[34]+e[13]*e[32]*e[26]+e[13]*e[23]*e[35]+e[10]*e[27]*e[24]+e[10]*e[18]*e[33]+e[10]*e[28]*e[25]+e[10]*e[19]*e[34]+e[10]*e[29]*e[26]+e[10]*e[20]*e[35]+e[22]*e[30]*e[15]+e[22]*e[12]*e[33]+e[22]*e[32]*e[17]+e[22]*e[14]*e[35]+e[22]*e[31]*e[16]+e[31]*e[21]*e[15]+e[31]*e[12]*e[24]+e[31]*e[23]*e[17]+e[31]*e[14]*e[26]-1.*e[16]*e[27]*e[18]+e[16]*e[33]*e[24]-1.*e[16]*e[30]*e[21]-1.*e[16]*e[29]*e[20]+e[16]*e[35]*e[26]-1.*e[16]*e[32]*e[23]+e[16]*e[28]*e[19]+3.*e[16]*e[34]*e[25]+e[19]*e[27]*e[15]+e[19]*e[9]*e[33]+e[19]*e[29]*e[17];
A[45]=e[4]*e[27]*e[3]+e[4]*e[0]*e[30]+e[4]*e[29]*e[5]+e[4]*e[2]*e[32]+e[31]*e[0]*e[3]+e[31]*e[2]*e[5]+e[7]*e[27]*e[6]+e[7]*e[0]*e[33]+e[7]*e[29]*e[8]+e[7]*e[2]*e[35]+e[34]*e[0]*e[6]+e[34]*e[2]*e[8]+e[1]*e[27]*e[0]+e[1]*e[29]*e[2]+e[1]*e[34]*e[7]-1.*e[1]*e[32]*e[5]-1.*e[1]*e[33]*e[6]-1.*e[1]*e[30]*e[3]-1.*e[1]*e[35]*e[8]+e[1]*e[31]*e[4]+1.500000000*e[28]*ep2[1]+.5000000000*e[28]*ep2[4]+.5000000000*e[28]*ep2[0]-.5000000000*e[28]*ep2[6]-.5000000000*e[28]*ep2[5]+.5000000000*e[28]*ep2[7]-.5000000000*e[28]*ep2[3]+.5000000000*e[28]*ep2[2]-.5000000000*e[28]*ep2[8];
A[191]=-1.*e[35]*e[10]*e[1]-1.*e[35]*e[13]*e[4]+e[35]*e[16]*e[7]+e[35]*e[15]*e[6]-1.*e[35]*e[9]*e[0]-1.*e[35]*e[12]*e[3]+e[32]*e[12]*e[6]+e[32]*e[3]*e[15]+e[32]*e[13]*e[7]+e[32]*e[4]*e[16]-1.*e[8]*e[27]*e[9]-1.*e[8]*e[30]*e[12]-1.*e[8]*e[28]*e[10]-1.*e[8]*e[31]*e[13]+e[8]*e[29]*e[11]+e[11]*e[27]*e[6]+e[11]*e[0]*e[33]+e[11]*e[28]*e[7]+e[11]*e[1]*e[34]+e[29]*e[9]*e[6]+e[29]*e[0]*e[15]+e[29]*e[10]*e[7]+e[29]*e[1]*e[16]+e[5]*e[30]*e[15]+e[5]*e[12]*e[33]+e[5]*e[32]*e[17]+e[5]*e[14]*e[35]+e[5]*e[31]*e[16]+e[5]*e[13]*e[34]+e[8]*e[33]*e[15]+3.*e[8]*e[35]*e[17]+e[8]*e[34]*e[16]+e[2]*e[27]*e[15]+e[2]*e[9]*e[33]+e[2]*e[29]*e[17]+e[2]*e[11]*e[35]+e[2]*e[28]*e[16]+e[2]*e[10]*e[34]-1.*e[17]*e[27]*e[0]+e[17]*e[34]*e[7]+e[17]*e[33]*e[6]-1.*e[17]*e[30]*e[3]-1.*e[17]*e[28]*e[1]-1.*e[17]*e[31]*e[4]+e[14]*e[30]*e[6]+e[14]*e[3]*e[33]+e[14]*e[31]*e[7]+e[14]*e[4]*e[34]+e[14]*e[32]*e[8];
A[44]=e[19]*e[11]*e[2]+e[4]*e[18]*e[12]+e[4]*e[9]*e[21]+e[4]*e[20]*e[14]+e[4]*e[11]*e[23]+e[4]*e[19]*e[13]+e[4]*e[10]*e[22]+e[7]*e[18]*e[15]+e[7]*e[9]*e[24]+e[7]*e[20]*e[17]+e[7]*e[11]*e[26]+e[7]*e[19]*e[16]+e[7]*e[10]*e[25]+e[1]*e[18]*e[9]+e[1]*e[20]*e[11]-1.*e[10]*e[21]*e[3]-1.*e[10]*e[26]*e[8]-1.*e[10]*e[23]*e[5]-1.*e[10]*e[24]*e[6]+e[13]*e[18]*e[3]+e[13]*e[0]*e[21]+e[13]*e[1]*e[22]+e[13]*e[20]*e[5]+e[13]*e[2]*e[23]-1.*e[19]*e[15]*e[6]-1.*e[19]*e[14]*e[5]-1.*e[19]*e[12]*e[3]-1.*e[19]*e[17]*e[8]+e[22]*e[9]*e[3]+e[22]*e[0]*e[12]+e[22]*e[11]*e[5]+e[22]*e[2]*e[14]+e[16]*e[18]*e[6]+e[16]*e[0]*e[24]+e[16]*e[1]*e[25]+e[16]*e[20]*e[8]+e[16]*e[2]*e[26]-1.*e[1]*e[23]*e[14]-1.*e[1]*e[24]*e[15]-1.*e[1]*e[26]*e[17]-1.*e[1]*e[21]*e[12]+e[25]*e[9]*e[6]+e[25]*e[0]*e[15]+e[25]*e[11]*e[8]+e[25]*e[2]*e[17]+e[10]*e[18]*e[0]+3.*e[10]*e[19]*e[1]+e[10]*e[20]*e[2]+e[19]*e[9]*e[0];
A[190]=.5000000000*ep2[23]*e[26]+.5000000000*e[26]*ep2[25]+.5000000000*ep2[20]*e[26]-.5000000000*e[26]*ep2[18]+.5000000000*ep3[26]+.5000000000*e[26]*ep2[24]+e[20]*e[19]*e[25]-.5000000000*e[26]*ep2[19]-.5000000000*e[26]*ep2[21]+e[20]*e[18]*e[24]-.5000000000*e[26]*ep2[22]+e[23]*e[21]*e[24]+e[23]*e[22]*e[25];
A[47]=e[16]*e[9]*e[33]+e[16]*e[29]*e[17]+e[16]*e[11]*e[35]+e[16]*e[10]*e[34]+e[34]*e[11]*e[17]+e[34]*e[9]*e[15]-1.*e[10]*e[30]*e[12]-1.*e[10]*e[32]*e[14]-1.*e[10]*e[33]*e[15]-1.*e[10]*e[35]*e[17]+e[10]*e[27]*e[9]+e[10]*e[29]*e[11]+e[13]*e[27]*e[12]+e[13]*e[9]*e[30]+e[13]*e[29]*e[14]+e[13]*e[11]*e[32]+e[13]*e[10]*e[31]+e[31]*e[11]*e[14]+e[31]*e[9]*e[12]+e[16]*e[27]*e[15]+1.500000000*e[28]*ep2[10]+.5000000000*e[28]*ep2[16]+.5000000000*e[28]*ep2[9]+.5000000000*e[28]*ep2[11]-.5000000000*e[28]*ep2[12]-.5000000000*e[28]*ep2[15]-.5000000000*e[28]*ep2[17]-.5000000000*e[28]*ep2[14]+.5000000000*e[28]*ep2[13];
A[189]=.5000000000*ep2[20]*e[35]+.5000000000*ep2[23]*e[35]+1.500000000*e[35]*ep2[26]+.5000000000*e[35]*ep2[25]+.5000000000*e[35]*ep2[24]-.5000000000*e[35]*ep2[18]-.5000000000*e[35]*ep2[19]-.5000000000*e[35]*ep2[22]-.5000000000*e[35]*ep2[21]+e[20]*e[27]*e[24]+e[20]*e[18]*e[33]+e[20]*e[28]*e[25]+e[20]*e[19]*e[34]+e[20]*e[29]*e[26]+e[29]*e[19]*e[25]+e[29]*e[18]*e[24]+e[23]*e[30]*e[24]+e[23]*e[21]*e[33]+e[23]*e[31]*e[25]+e[23]*e[22]*e[34]+e[23]*e[32]*e[26]+e[32]*e[22]*e[25]+e[32]*e[21]*e[24]+e[26]*e[33]*e[24]+e[26]*e[34]*e[25]-1.*e[26]*e[27]*e[18]-1.*e[26]*e[30]*e[21]-1.*e[26]*e[31]*e[22]-1.*e[26]*e[28]*e[19];
A[46]=e[4]*e[2]*e[5]+.5000000000*e[1]*ep2[0]-.5000000000*e[1]*ep2[6]+e[7]*e[0]*e[6]+.5000000000*e[1]*ep2[7]+.5000000000*e[1]*ep2[4]-.5000000000*e[1]*ep2[8]+.5000000000*e[1]*ep2[2]-.5000000000*e[1]*ep2[3]+.5000000000*ep3[1]+e[7]*e[2]*e[8]-.5000000000*e[1]*ep2[5]+e[4]*e[0]*e[3];
A[188]=-.5000000000*e[17]*ep2[13]-.5000000000*e[17]*ep2[9]+.5000000000*e[17]*ep2[16]+.5000000000*e[17]*ep2[15]+.5000000000*ep3[17]-.5000000000*e[17]*ep2[10]+e[14]*e[13]*e[16]+e[14]*e[12]*e[15]+.5000000000*ep2[14]*e[17]+e[11]*e[10]*e[16]-.5000000000*e[17]*ep2[12]+.5000000000*ep2[11]*e[17]+e[11]*e[9]*e[15];
A[41]=e[4]*e[27]*e[30]+e[4]*e[29]*e[32]+e[4]*e[28]*e[31]+e[31]*e[27]*e[3]+e[31]*e[0]*e[30]+e[31]*e[29]*e[5]+e[31]*e[2]*e[32]+e[7]*e[27]*e[33]+e[7]*e[29]*e[35]+e[7]*e[28]*e[34]+e[28]*e[27]*e[0]+e[28]*e[29]*e[2]+e[34]*e[27]*e[6]+e[34]*e[0]*e[33]+e[34]*e[29]*e[8]+e[34]*e[2]*e[35]-1.*e[28]*e[32]*e[5]-1.*e[28]*e[33]*e[6]-1.*e[28]*e[30]*e[3]-1.*e[28]*e[35]*e[8]+.5000000000*e[1]*ep2[27]+.5000000000*e[1]*ep2[29]+1.500000000*e[1]*ep2[28]+.5000000000*e[1]*ep2[31]-.5000000000*e[1]*ep2[32]-.5000000000*e[1]*ep2[33]-.5000000000*e[1]*ep2[30]+.5000000000*e[1]*ep2[34]-.5000000000*e[1]*ep2[35];
A[187]=.5000000000*ep2[11]*e[35]+.5000000000*e[35]*ep2[16]-.5000000000*e[35]*ep2[9]-.5000000000*e[35]*ep2[12]+.5000000000*e[35]*ep2[15]+1.500000000*e[35]*ep2[17]-.5000000000*e[35]*ep2[10]+.5000000000*e[35]*ep2[14]-.5000000000*e[35]*ep2[13]+e[11]*e[27]*e[15]+e[11]*e[9]*e[33]+e[11]*e[29]*e[17]+e[11]*e[28]*e[16]+e[11]*e[10]*e[34]+e[29]*e[9]*e[15]+e[29]*e[10]*e[16]+e[14]*e[30]*e[15]+e[14]*e[12]*e[33]+e[14]*e[32]*e[17]+e[14]*e[31]*e[16]+e[14]*e[13]*e[34]+e[32]*e[12]*e[15]+e[32]*e[13]*e[16]+e[17]*e[33]*e[15]+e[17]*e[34]*e[16]-1.*e[17]*e[27]*e[9]-1.*e[17]*e[30]*e[12]-1.*e[17]*e[28]*e[10]-1.*e[17]*e[31]*e[13];
A[40]=e[34]*e[27]*e[33]+e[34]*e[29]*e[35]-.5000000000*e[28]*ep2[30]-.5000000000*e[28]*ep2[35]+.5000000000*ep3[28]+.5000000000*e[28]*ep2[27]+.5000000000*e[28]*ep2[29]+e[31]*e[27]*e[30]+e[31]*e[29]*e[32]-.5000000000*e[28]*ep2[32]-.5000000000*e[28]*ep2[33]+.5000000000*e[28]*ep2[31]+.5000000000*e[28]*ep2[34];
A[186]=.5000000000*ep2[5]*e[8]+e[2]*e[0]*e[6]+.5000000000*ep2[2]*e[8]+.5000000000*ep3[8]-.5000000000*e[8]*ep2[0]+e[5]*e[4]*e[7]+e[5]*e[3]*e[6]+.5000000000*e[8]*ep2[7]+e[2]*e[1]*e[7]-.5000000000*e[8]*ep2[1]-.5000000000*e[8]*ep2[4]-.5000000000*e[8]*ep2[3]+.5000000000*e[8]*ep2[6];
A[43]=e[28]*e[27]*e[9]+e[28]*e[29]*e[11]-1.*e[28]*e[30]*e[12]+e[28]*e[31]*e[13]-1.*e[28]*e[32]*e[14]-1.*e[28]*e[33]*e[15]-1.*e[28]*e[35]*e[17]+e[31]*e[27]*e[12]+e[31]*e[9]*e[30]+e[31]*e[29]*e[14]+e[31]*e[11]*e[32]+e[13]*e[27]*e[30]+e[13]*e[29]*e[32]+e[16]*e[27]*e[33]+e[16]*e[29]*e[35]+e[34]*e[27]*e[15]+e[34]*e[9]*e[33]+e[34]*e[29]*e[17]+e[34]*e[11]*e[35]+e[34]*e[28]*e[16]+.5000000000*e[10]*ep2[27]+.5000000000*e[10]*ep2[29]+1.500000000*e[10]*ep2[28]-.5000000000*e[10]*ep2[32]+.5000000000*e[10]*ep2[31]-.5000000000*e[10]*ep2[33]-.5000000000*e[10]*ep2[30]+.5000000000*e[10]*ep2[34]-.5000000000*e[10]*ep2[35];
A[185]=-.5000000000*e[35]*ep2[1]+.5000000000*e[35]*ep2[7]-.5000000000*e[35]*ep2[3]+.5000000000*ep2[2]*e[35]+1.500000000*e[35]*ep2[8]-.5000000000*e[35]*ep2[4]-.5000000000*e[35]*ep2[0]+.5000000000*e[35]*ep2[6]+.5000000000*e[35]*ep2[5]+e[2]*e[27]*e[6]+e[2]*e[0]*e[33]+e[2]*e[28]*e[7]+e[2]*e[1]*e[34]+e[2]*e[29]*e[8]-1.*e[8]*e[27]*e[0]+e[8]*e[34]*e[7]+e[8]*e[32]*e[5]+e[8]*e[33]*e[6]-1.*e[8]*e[30]*e[3]-1.*e[8]*e[28]*e[1]-1.*e[8]*e[31]*e[4]+e[29]*e[1]*e[7]+e[29]*e[0]*e[6]+e[5]*e[30]*e[6]+e[5]*e[3]*e[33]+e[5]*e[31]*e[7]+e[5]*e[4]*e[34]+e[32]*e[4]*e[7]+e[32]*e[3]*e[6];
A[42]=e[28]*e[27]*e[18]+e[28]*e[29]*e[20]+e[22]*e[27]*e[30]+e[22]*e[29]*e[32]+e[22]*e[28]*e[31]+e[31]*e[27]*e[21]+e[31]*e[18]*e[30]+e[31]*e[29]*e[23]+e[31]*e[20]*e[32]+e[25]*e[27]*e[33]+e[25]*e[29]*e[35]+e[25]*e[28]*e[34]+e[34]*e[27]*e[24]+e[34]*e[18]*e[33]+e[34]*e[29]*e[26]+e[34]*e[20]*e[35]-1.*e[28]*e[33]*e[24]-1.*e[28]*e[30]*e[21]-1.*e[28]*e[35]*e[26]-1.*e[28]*e[32]*e[23]-.5000000000*e[19]*ep2[33]-.5000000000*e[19]*ep2[30]-.5000000000*e[19]*ep2[35]+.5000000000*e[19]*ep2[27]+.5000000000*e[19]*ep2[29]+1.500000000*e[19]*ep2[28]+.5000000000*e[19]*ep2[31]+.5000000000*e[19]*ep2[34]-.5000000000*e[19]*ep2[32];
A[184]=e[23]*e[3]*e[15]-1.*e[17]*e[19]*e[1]-1.*e[17]*e[22]*e[4]-1.*e[17]*e[18]*e[0]+e[17]*e[25]*e[7]+e[17]*e[24]*e[6]+e[14]*e[21]*e[6]+e[14]*e[3]*e[24]+e[14]*e[22]*e[7]+e[14]*e[4]*e[25]+e[14]*e[23]*e[8]-1.*e[26]*e[10]*e[1]-1.*e[26]*e[13]*e[4]+e[26]*e[16]*e[7]+e[26]*e[15]*e[6]-1.*e[26]*e[9]*e[0]-1.*e[26]*e[12]*e[3]+e[23]*e[12]*e[6]+e[11]*e[18]*e[6]+e[11]*e[0]*e[24]+e[11]*e[19]*e[7]+e[11]*e[1]*e[25]+e[11]*e[20]*e[8]+e[11]*e[2]*e[26]+e[20]*e[9]*e[6]+e[20]*e[0]*e[15]+e[20]*e[10]*e[7]+e[20]*e[1]*e[16]+e[20]*e[2]*e[17]+e[5]*e[21]*e[15]+e[5]*e[12]*e[24]+e[5]*e[23]*e[17]+e[5]*e[14]*e[26]+e[5]*e[22]*e[16]+e[5]*e[13]*e[25]+e[8]*e[24]*e[15]+3.*e[8]*e[26]*e[17]+e[8]*e[25]*e[16]+e[2]*e[18]*e[15]+e[2]*e[9]*e[24]+e[2]*e[19]*e[16]+e[2]*e[10]*e[25]-1.*e[17]*e[21]*e[3]+e[23]*e[4]*e[16]+e[23]*e[13]*e[7]-1.*e[8]*e[18]*e[9]-1.*e[8]*e[21]*e[12]-1.*e[8]*e[19]*e[10]-1.*e[8]*e[22]*e[13];
A[54]=e[13]*e[18]*e[12]+e[13]*e[9]*e[21]+e[13]*e[20]*e[14]+e[13]*e[11]*e[23]+e[13]*e[10]*e[22]+e[22]*e[11]*e[14]+e[22]*e[9]*e[12]+e[16]*e[18]*e[15]+e[16]*e[9]*e[24]+e[16]*e[20]*e[17]+e[16]*e[11]*e[26]+e[16]*e[10]*e[25]+e[25]*e[11]*e[17]+e[25]*e[9]*e[15]-1.*e[10]*e[23]*e[14]-1.*e[10]*e[24]*e[15]-1.*e[10]*e[26]*e[17]+e[10]*e[20]*e[11]+e[10]*e[18]*e[9]-1.*e[10]*e[21]*e[12]+.5000000000*e[19]*ep2[11]+.5000000000*e[19]*ep2[9]+1.500000000*e[19]*ep2[10]+.5000000000*e[19]*ep2[13]+.5000000000*e[19]*ep2[16]-.5000000000*e[19]*ep2[12]-.5000000000*e[19]*ep2[15]-.5000000000*e[19]*ep2[17]-.5000000000*e[19]*ep2[14];
A[164]=e[10]*e[18]*e[6]+e[10]*e[0]*e[24]+e[10]*e[19]*e[7]+e[10]*e[1]*e[25]+e[10]*e[20]*e[8]+e[10]*e[2]*e[26]+e[19]*e[9]*e[6]+e[19]*e[0]*e[15]+e[19]*e[1]*e[16]+e[19]*e[11]*e[8]+e[19]*e[2]*e[17]+e[4]*e[21]*e[15]+e[4]*e[12]*e[24]+e[4]*e[23]*e[17]+e[4]*e[14]*e[26]+e[4]*e[22]*e[16]+e[4]*e[13]*e[25]+e[7]*e[24]*e[15]+e[7]*e[26]*e[17]+3.*e[7]*e[25]*e[16]+e[1]*e[18]*e[15]+e[1]*e[9]*e[24]+e[1]*e[20]*e[17]+e[1]*e[11]*e[26]-1.*e[16]*e[21]*e[3]+e[16]*e[26]*e[8]-1.*e[16]*e[20]*e[2]-1.*e[16]*e[18]*e[0]-1.*e[16]*e[23]*e[5]+e[16]*e[24]*e[6]+e[13]*e[21]*e[6]+e[13]*e[3]*e[24]+e[13]*e[22]*e[7]+e[13]*e[23]*e[8]+e[13]*e[5]*e[26]-1.*e[25]*e[11]*e[2]+e[25]*e[15]*e[6]-1.*e[25]*e[9]*e[0]-1.*e[25]*e[14]*e[5]-1.*e[25]*e[12]*e[3]+e[25]*e[17]*e[8]+e[22]*e[12]*e[6]+e[22]*e[3]*e[15]+e[22]*e[14]*e[8]+e[22]*e[5]*e[17]-1.*e[7]*e[23]*e[14]-1.*e[7]*e[20]*e[11]-1.*e[7]*e[18]*e[9]-1.*e[7]*e[21]*e[12];
A[55]=e[13]*e[9]*e[3]+e[13]*e[0]*e[12]+e[13]*e[10]*e[4]+e[13]*e[11]*e[5]+e[13]*e[2]*e[14]+e[16]*e[9]*e[6]+e[16]*e[0]*e[15]+e[16]*e[10]*e[7]+e[16]*e[11]*e[8]+e[16]*e[2]*e[17]+e[7]*e[11]*e[17]+e[7]*e[9]*e[15]+e[4]*e[11]*e[14]+e[4]*e[9]*e[12]+e[10]*e[9]*e[0]+e[10]*e[11]*e[2]-1.*e[10]*e[15]*e[6]-1.*e[10]*e[14]*e[5]-1.*e[10]*e[12]*e[3]-1.*e[10]*e[17]*e[8]+.5000000000*e[1]*ep2[11]+.5000000000*e[1]*ep2[9]+1.500000000*e[1]*ep2[10]-.5000000000*e[1]*ep2[12]-.5000000000*e[1]*ep2[15]-.5000000000*e[1]*ep2[17]-.5000000000*e[1]*ep2[14]+.5000000000*e[1]*ep2[13]+.5000000000*e[1]*ep2[16];
A[165]=e[1]*e[27]*e[6]+e[1]*e[0]*e[33]+e[1]*e[28]*e[7]+e[1]*e[29]*e[8]+e[1]*e[2]*e[35]-1.*e[7]*e[27]*e[0]-1.*e[7]*e[32]*e[5]+e[7]*e[33]*e[6]-1.*e[7]*e[30]*e[3]+e[7]*e[35]*e[8]-1.*e[7]*e[29]*e[2]+e[7]*e[31]*e[4]+e[28]*e[0]*e[6]+e[28]*e[2]*e[8]+e[4]*e[30]*e[6]+e[4]*e[3]*e[33]+e[4]*e[32]*e[8]+e[4]*e[5]*e[35]+e[31]*e[3]*e[6]+e[31]*e[5]*e[8]+.5000000000*ep2[1]*e[34]+1.500000000*e[34]*ep2[7]+.5000000000*e[34]*ep2[4]-.5000000000*e[34]*ep2[0]+.5000000000*e[34]*ep2[6]-.5000000000*e[34]*ep2[5]-.5000000000*e[34]*ep2[3]-.5000000000*e[34]*ep2[2]+.5000000000*e[34]*ep2[8];
A[52]=e[4]*e[18]*e[3]+e[4]*e[0]*e[21]+e[4]*e[1]*e[22]+e[4]*e[20]*e[5]+e[4]*e[2]*e[23]+e[22]*e[0]*e[3]+e[22]*e[2]*e[5]+e[7]*e[18]*e[6]+e[7]*e[0]*e[24]+e[7]*e[1]*e[25]+e[7]*e[20]*e[8]+e[7]*e[2]*e[26]+e[25]*e[0]*e[6]+e[25]*e[2]*e[8]+e[1]*e[18]*e[0]+e[1]*e[20]*e[2]-1.*e[1]*e[21]*e[3]-1.*e[1]*e[26]*e[8]-1.*e[1]*e[23]*e[5]-1.*e[1]*e[24]*e[6]+.5000000000*e[19]*ep2[4]+.5000000000*e[19]*ep2[0]-.5000000000*e[19]*ep2[6]-.5000000000*e[19]*ep2[5]+1.500000000*e[19]*ep2[1]+.5000000000*e[19]*ep2[7]-.5000000000*e[19]*ep2[3]+.5000000000*e[19]*ep2[2]-.5000000000*e[19]*ep2[8];
A[166]=-.5000000000*e[7]*ep2[0]+e[4]*e[5]*e[8]+.5000000000*ep2[4]*e[7]-.5000000000*e[7]*ep2[2]+.5000000000*e[7]*ep2[8]-.5000000000*e[7]*ep2[5]+.5000000000*e[7]*ep2[6]+e[1]*e[0]*e[6]+.5000000000*ep3[7]+e[4]*e[3]*e[6]+e[1]*e[2]*e[8]-.5000000000*e[7]*ep2[3]+.5000000000*ep2[1]*e[7];
A[53]=-1.*e[1]*e[32]*e[23]-1.*e[19]*e[32]*e[5]-1.*e[19]*e[33]*e[6]-1.*e[19]*e[30]*e[3]-1.*e[19]*e[35]*e[8]-1.*e[28]*e[21]*e[3]-1.*e[28]*e[26]*e[8]-1.*e[28]*e[23]*e[5]-1.*e[28]*e[24]*e[6]+e[7]*e[27]*e[24]+e[7]*e[18]*e[33]+e[7]*e[29]*e[26]+e[7]*e[20]*e[35]+e[22]*e[27]*e[3]+e[22]*e[0]*e[30]+e[22]*e[29]*e[5]+e[22]*e[2]*e[32]+e[31]*e[18]*e[3]+e[31]*e[0]*e[21]+e[31]*e[20]*e[5]+e[31]*e[2]*e[23]+e[25]*e[27]*e[6]+e[25]*e[0]*e[33]+e[25]*e[28]*e[7]+e[25]*e[1]*e[34]+e[25]*e[29]*e[8]+e[25]*e[2]*e[35]+e[34]*e[18]*e[6]+e[34]*e[0]*e[24]+e[34]*e[19]*e[7]+e[34]*e[20]*e[8]+e[34]*e[2]*e[26]+e[1]*e[27]*e[18]+3.*e[1]*e[28]*e[19]+e[1]*e[29]*e[20]+e[19]*e[27]*e[0]+e[19]*e[29]*e[2]+e[28]*e[18]*e[0]+e[28]*e[20]*e[2]+e[4]*e[27]*e[21]+e[4]*e[18]*e[30]+e[4]*e[28]*e[22]+e[4]*e[19]*e[31]+e[4]*e[29]*e[23]+e[4]*e[20]*e[32]-1.*e[1]*e[33]*e[24]-1.*e[1]*e[30]*e[21]-1.*e[1]*e[35]*e[26]+e[1]*e[31]*e[22];
A[167]=e[10]*e[27]*e[15]+e[10]*e[9]*e[33]+e[10]*e[29]*e[17]+e[10]*e[11]*e[35]+e[10]*e[28]*e[16]+e[28]*e[11]*e[17]+e[28]*e[9]*e[15]+e[13]*e[30]*e[15]+e[13]*e[12]*e[33]+e[13]*e[32]*e[17]+e[13]*e[14]*e[35]+e[13]*e[31]*e[16]+e[31]*e[14]*e[17]+e[31]*e[12]*e[15]+e[16]*e[33]*e[15]+e[16]*e[35]*e[17]-1.*e[16]*e[27]*e[9]-1.*e[16]*e[30]*e[12]-1.*e[16]*e[32]*e[14]-1.*e[16]*e[29]*e[11]+.5000000000*ep2[10]*e[34]+1.500000000*e[34]*ep2[16]-.5000000000*e[34]*ep2[9]-.5000000000*e[34]*ep2[11]-.5000000000*e[34]*ep2[12]+.5000000000*e[34]*ep2[15]+.5000000000*e[34]*ep2[17]-.5000000000*e[34]*ep2[14]+.5000000000*e[34]*ep2[13];
A[50]=.5000000000*e[19]*ep2[18]+.5000000000*e[19]*ep2[25]+.5000000000*e[19]*ep2[22]+e[25]*e[20]*e[26]-.5000000000*e[19]*ep2[21]+.5000000000*e[19]*ep2[20]-.5000000000*e[19]*ep2[26]-.5000000000*e[19]*ep2[23]-.5000000000*e[19]*ep2[24]+.5000000000*ep3[19]+e[22]*e[20]*e[23]+e[25]*e[18]*e[24]+e[22]*e[18]*e[21];
A[160]=.5000000000*e[34]*ep2[33]+.5000000000*e[34]*ep2[35]-.5000000000*e[34]*ep2[27]-.5000000000*e[34]*ep2[32]-.5000000000*e[34]*ep2[29]-.5000000000*e[34]*ep2[30]+.5000000000*ep2[28]*e[34]+e[31]*e[30]*e[33]+e[31]*e[32]*e[35]+e[28]*e[27]*e[33]+.5000000000*ep3[34]+e[28]*e[29]*e[35]+.5000000000*ep2[31]*e[34];
A[51]=e[4]*e[28]*e[13]+e[4]*e[10]*e[31]+e[7]*e[27]*e[15]+e[7]*e[9]*e[33]+e[7]*e[29]*e[17]+e[7]*e[11]*e[35]+e[7]*e[28]*e[16]+e[7]*e[10]*e[34]+e[1]*e[27]*e[9]+e[1]*e[29]*e[11]+3.*e[1]*e[28]*e[10]+e[10]*e[27]*e[0]-1.*e[10]*e[32]*e[5]-1.*e[10]*e[33]*e[6]-1.*e[10]*e[30]*e[3]-1.*e[10]*e[35]*e[8]+e[10]*e[29]*e[2]+e[13]*e[27]*e[3]+e[13]*e[0]*e[30]+e[13]*e[1]*e[31]+e[13]*e[29]*e[5]+e[13]*e[2]*e[32]+e[28]*e[11]*e[2]-1.*e[28]*e[15]*e[6]+e[28]*e[9]*e[0]-1.*e[28]*e[14]*e[5]-1.*e[28]*e[12]*e[3]-1.*e[28]*e[17]*e[8]+e[31]*e[9]*e[3]+e[31]*e[0]*e[12]+e[31]*e[11]*e[5]+e[31]*e[2]*e[14]+e[16]*e[27]*e[6]+e[16]*e[0]*e[33]+e[16]*e[1]*e[34]+e[16]*e[29]*e[8]+e[16]*e[2]*e[35]-1.*e[1]*e[30]*e[12]-1.*e[1]*e[32]*e[14]-1.*e[1]*e[33]*e[15]-1.*e[1]*e[35]*e[17]+e[34]*e[9]*e[6]+e[34]*e[0]*e[15]+e[34]*e[11]*e[8]+e[34]*e[2]*e[17]+e[4]*e[27]*e[12]+e[4]*e[9]*e[30]+e[4]*e[29]*e[14]+e[4]*e[11]*e[32];
A[161]=e[4]*e[30]*e[33]+e[4]*e[32]*e[35]+e[4]*e[31]*e[34]+e[31]*e[30]*e[6]+e[31]*e[3]*e[33]+e[31]*e[32]*e[8]+e[31]*e[5]*e[35]+e[28]*e[27]*e[6]+e[28]*e[0]*e[33]+e[28]*e[29]*e[8]+e[28]*e[2]*e[35]+e[34]*e[33]*e[6]+e[34]*e[35]*e[8]-1.*e[34]*e[27]*e[0]-1.*e[34]*e[32]*e[5]-1.*e[34]*e[30]*e[3]-1.*e[34]*e[29]*e[2]+e[1]*e[27]*e[33]+e[1]*e[29]*e[35]+e[1]*e[28]*e[34]+.5000000000*ep2[31]*e[7]-.5000000000*e[7]*ep2[27]-.5000000000*e[7]*ep2[32]+.5000000000*e[7]*ep2[28]-.5000000000*e[7]*ep2[29]+.5000000000*e[7]*ep2[33]-.5000000000*e[7]*ep2[30]+1.500000000*e[7]*ep2[34]+.5000000000*e[7]*ep2[35];
A[48]=-.5000000000*e[10]*ep2[14]-.5000000000*e[10]*ep2[17]-.5000000000*e[10]*ep2[15]+e[13]*e[11]*e[14]+e[16]*e[11]*e[17]+.5000000000*e[10]*ep2[13]+e[13]*e[9]*e[12]-.5000000000*e[10]*ep2[12]+.5000000000*ep3[10]+e[16]*e[9]*e[15]+.5000000000*e[10]*ep2[16]+.5000000000*e[10]*ep2[11]+.5000000000*e[10]*ep2[9];
A[162]=e[22]*e[32]*e[35]+e[22]*e[31]*e[34]+e[31]*e[30]*e[24]+e[31]*e[21]*e[33]+e[31]*e[32]*e[26]+e[31]*e[23]*e[35]+e[34]*e[33]*e[24]+e[34]*e[35]*e[26]-1.*e[34]*e[27]*e[18]-1.*e[34]*e[30]*e[21]-1.*e[34]*e[29]*e[20]-1.*e[34]*e[32]*e[23]+e[19]*e[27]*e[33]+e[19]*e[29]*e[35]+e[19]*e[28]*e[34]+e[28]*e[27]*e[24]+e[28]*e[18]*e[33]+e[28]*e[29]*e[26]+e[28]*e[20]*e[35]+e[22]*e[30]*e[33]+.5000000000*ep2[28]*e[25]+.5000000000*ep2[31]*e[25]+.5000000000*e[25]*ep2[33]+.5000000000*e[25]*ep2[35]+1.500000000*e[25]*ep2[34]-.5000000000*e[25]*ep2[27]-.5000000000*e[25]*ep2[32]-.5000000000*e[25]*ep2[29]-.5000000000*e[25]*ep2[30];
A[49]=-1.*e[19]*e[35]*e[26]-1.*e[19]*e[32]*e[23]+e[19]*e[27]*e[18]+e[19]*e[29]*e[20]+e[22]*e[27]*e[21]+e[22]*e[18]*e[30]+e[22]*e[19]*e[31]+e[22]*e[29]*e[23]+e[22]*e[20]*e[32]+e[31]*e[18]*e[21]+e[31]*e[20]*e[23]+e[25]*e[27]*e[24]+e[25]*e[18]*e[33]+e[25]*e[19]*e[34]+e[25]*e[29]*e[26]+e[25]*e[20]*e[35]+e[34]*e[18]*e[24]+e[34]*e[20]*e[26]-1.*e[19]*e[33]*e[24]-1.*e[19]*e[30]*e[21]+1.500000000*e[28]*ep2[19]+.5000000000*e[28]*ep2[18]+.5000000000*e[28]*ep2[20]+.5000000000*e[28]*ep2[22]+.5000000000*e[28]*ep2[25]-.5000000000*e[28]*ep2[26]-.5000000000*e[28]*ep2[23]-.5000000000*e[28]*ep2[24]-.5000000000*e[28]*ep2[21];
A[163]=e[10]*e[27]*e[33]+e[10]*e[29]*e[35]+e[10]*e[28]*e[34]+e[34]*e[33]*e[15]+e[34]*e[35]*e[17]+e[28]*e[27]*e[15]+e[28]*e[9]*e[33]+e[28]*e[29]*e[17]+e[28]*e[11]*e[35]-1.*e[34]*e[27]*e[9]-1.*e[34]*e[30]*e[12]+e[34]*e[31]*e[13]-1.*e[34]*e[32]*e[14]-1.*e[34]*e[29]*e[11]+e[31]*e[30]*e[15]+e[31]*e[12]*e[33]+e[31]*e[32]*e[17]+e[31]*e[14]*e[35]+e[13]*e[30]*e[33]+e[13]*e[32]*e[35]-.5000000000*e[16]*ep2[27]-.5000000000*e[16]*ep2[32]+.5000000000*e[16]*ep2[28]-.5000000000*e[16]*ep2[29]+.5000000000*e[16]*ep2[31]+.5000000000*e[16]*ep2[33]-.5000000000*e[16]*ep2[30]+1.500000000*e[16]*ep2[34]+.5000000000*e[16]*ep2[35];
A[63]=e[29]*e[32]*e[14]-1.*e[29]*e[33]*e[15]-1.*e[29]*e[34]*e[16]+e[32]*e[27]*e[12]+e[32]*e[9]*e[30]+e[32]*e[28]*e[13]+e[32]*e[10]*e[31]+e[14]*e[27]*e[30]+e[14]*e[28]*e[31]+e[17]*e[27]*e[33]+e[17]*e[28]*e[34]+e[35]*e[27]*e[15]+e[35]*e[9]*e[33]+e[35]*e[29]*e[17]+e[35]*e[28]*e[16]+e[35]*e[10]*e[34]+e[29]*e[27]*e[9]+e[29]*e[28]*e[10]-1.*e[29]*e[30]*e[12]-1.*e[29]*e[31]*e[13]+.5000000000*e[11]*ep2[27]+1.500000000*e[11]*ep2[29]+.5000000000*e[11]*ep2[28]+.5000000000*e[11]*ep2[32]-.5000000000*e[11]*ep2[31]-.5000000000*e[11]*ep2[33]-.5000000000*e[11]*ep2[30]-.5000000000*e[11]*ep2[34]+.5000000000*e[11]*ep2[35];
A[173]=e[1]*e[20]*e[35]+e[19]*e[27]*e[6]+e[19]*e[0]*e[33]+e[19]*e[28]*e[7]+e[19]*e[29]*e[8]+e[19]*e[2]*e[35]+e[28]*e[18]*e[6]+e[28]*e[0]*e[24]+e[28]*e[20]*e[8]+e[28]*e[2]*e[26]+e[4]*e[30]*e[24]+e[4]*e[21]*e[33]+e[4]*e[31]*e[25]+e[4]*e[22]*e[34]+e[4]*e[32]*e[26]+e[4]*e[23]*e[35]-1.*e[7]*e[27]*e[18]+e[7]*e[33]*e[24]-1.*e[7]*e[30]*e[21]-1.*e[7]*e[29]*e[20]+e[7]*e[35]*e[26]+e[7]*e[31]*e[22]-1.*e[7]*e[32]*e[23]-1.*e[25]*e[27]*e[0]-1.*e[25]*e[32]*e[5]-1.*e[25]*e[30]*e[3]-1.*e[25]*e[29]*e[2]-1.*e[34]*e[21]*e[3]-1.*e[34]*e[20]*e[2]-1.*e[34]*e[18]*e[0]-1.*e[34]*e[23]*e[5]+e[22]*e[30]*e[6]+e[22]*e[3]*e[33]+e[22]*e[32]*e[8]+e[22]*e[5]*e[35]+e[31]*e[21]*e[6]+e[31]*e[3]*e[24]+e[31]*e[23]*e[8]+e[31]*e[5]*e[26]+e[34]*e[26]*e[8]+e[1]*e[27]*e[24]+e[1]*e[18]*e[33]+e[1]*e[28]*e[25]+e[1]*e[19]*e[34]+e[1]*e[29]*e[26]+e[34]*e[24]*e[6]+e[25]*e[33]*e[6]+3.*e[25]*e[34]*e[7]+e[25]*e[35]*e[8];
A[62]=.5000000000*e[20]*ep2[27]+1.500000000*e[20]*ep2[29]+.5000000000*e[20]*ep2[28]+.5000000000*e[20]*ep2[32]+.5000000000*e[20]*ep2[35]-.5000000000*e[20]*ep2[31]-.5000000000*e[20]*ep2[33]-.5000000000*e[20]*ep2[30]-.5000000000*e[20]*ep2[34]+e[29]*e[27]*e[18]+e[29]*e[28]*e[19]+e[23]*e[27]*e[30]+e[23]*e[29]*e[32]+e[23]*e[28]*e[31]+e[32]*e[27]*e[21]+e[32]*e[18]*e[30]+e[32]*e[28]*e[22]+e[32]*e[19]*e[31]+e[26]*e[27]*e[33]+e[26]*e[29]*e[35]+e[26]*e[28]*e[34]+e[35]*e[27]*e[24]+e[35]*e[18]*e[33]+e[35]*e[28]*e[25]+e[35]*e[19]*e[34]-1.*e[29]*e[33]*e[24]-1.*e[29]*e[30]*e[21]-1.*e[29]*e[31]*e[22]-1.*e[29]*e[34]*e[25];
A[172]=e[19]*e[1]*e[7]+e[19]*e[0]*e[6]+e[19]*e[2]*e[8]+e[4]*e[21]*e[6]+e[4]*e[3]*e[24]+e[4]*e[22]*e[7]+e[4]*e[23]*e[8]+e[4]*e[5]*e[26]+e[22]*e[3]*e[6]+e[22]*e[5]*e[8]+e[7]*e[24]*e[6]+e[7]*e[26]*e[8]+e[1]*e[18]*e[6]+e[1]*e[0]*e[24]+e[1]*e[20]*e[8]+e[1]*e[2]*e[26]-1.*e[7]*e[21]*e[3]-1.*e[7]*e[20]*e[2]-1.*e[7]*e[18]*e[0]-1.*e[7]*e[23]*e[5]+.5000000000*e[25]*ep2[4]-.5000000000*e[25]*ep2[0]+.5000000000*e[25]*ep2[6]-.5000000000*e[25]*ep2[5]+.5000000000*e[25]*ep2[1]+1.500000000*e[25]*ep2[7]-.5000000000*e[25]*ep2[3]-.5000000000*e[25]*ep2[2]+.5000000000*e[25]*ep2[8];
A[61]=e[5]*e[27]*e[30]+e[5]*e[29]*e[32]+e[5]*e[28]*e[31]+e[32]*e[27]*e[3]+e[32]*e[0]*e[30]+e[32]*e[28]*e[4]+e[32]*e[1]*e[31]+e[8]*e[27]*e[33]+e[8]*e[29]*e[35]+e[8]*e[28]*e[34]+e[29]*e[27]*e[0]+e[29]*e[28]*e[1]+e[35]*e[27]*e[6]+e[35]*e[0]*e[33]+e[35]*e[28]*e[7]+e[35]*e[1]*e[34]-1.*e[29]*e[34]*e[7]-1.*e[29]*e[33]*e[6]-1.*e[29]*e[30]*e[3]-1.*e[29]*e[31]*e[4]+.5000000000*e[2]*ep2[27]+1.500000000*e[2]*ep2[29]+.5000000000*e[2]*ep2[28]+.5000000000*e[2]*ep2[32]-.5000000000*e[2]*ep2[31]-.5000000000*e[2]*ep2[33]-.5000000000*e[2]*ep2[30]-.5000000000*e[2]*ep2[34]+.5000000000*e[2]*ep2[35];
A[175]=e[13]*e[12]*e[6]+e[13]*e[3]*e[15]+e[13]*e[4]*e[16]+e[13]*e[14]*e[8]+e[13]*e[5]*e[17]+e[16]*e[15]*e[6]+e[16]*e[17]*e[8]+e[1]*e[11]*e[17]+e[1]*e[9]*e[15]+e[1]*e[10]*e[16]+e[4]*e[14]*e[17]+e[4]*e[12]*e[15]+e[10]*e[9]*e[6]+e[10]*e[0]*e[15]+e[10]*e[11]*e[8]+e[10]*e[2]*e[17]-1.*e[16]*e[11]*e[2]-1.*e[16]*e[9]*e[0]-1.*e[16]*e[14]*e[5]-1.*e[16]*e[12]*e[3]+.5000000000*ep2[13]*e[7]+1.500000000*ep2[16]*e[7]+.5000000000*e[7]*ep2[17]+.5000000000*e[7]*ep2[15]-.5000000000*e[7]*ep2[9]-.5000000000*e[7]*ep2[11]-.5000000000*e[7]*ep2[12]+.5000000000*e[7]*ep2[10]-.5000000000*e[7]*ep2[14];
A[60]=.5000000000*e[29]*ep2[32]+.5000000000*e[29]*ep2[35]-.5000000000*e[29]*ep2[31]-.5000000000*e[29]*ep2[33]-.5000000000*e[29]*ep2[30]-.5000000000*e[29]*ep2[34]+e[32]*e[27]*e[30]+.5000000000*ep3[29]+.5000000000*e[29]*ep2[28]+e[35]*e[28]*e[34]+.5000000000*e[29]*ep2[27]+e[35]*e[27]*e[33]+e[32]*e[28]*e[31];
A[174]=-1.*e[16]*e[21]*e[12]+e[10]*e[18]*e[15]+e[10]*e[9]*e[24]+e[10]*e[20]*e[17]+e[10]*e[11]*e[26]+e[19]*e[11]*e[17]+e[19]*e[9]*e[15]+e[19]*e[10]*e[16]+e[13]*e[21]*e[15]+e[13]*e[12]*e[24]+e[13]*e[23]*e[17]+e[13]*e[14]*e[26]+e[13]*e[22]*e[16]+e[22]*e[14]*e[17]+e[22]*e[12]*e[15]+e[16]*e[24]*e[15]+e[16]*e[26]*e[17]-1.*e[16]*e[23]*e[14]-1.*e[16]*e[20]*e[11]-1.*e[16]*e[18]*e[9]+.5000000000*ep2[13]*e[25]+1.500000000*e[25]*ep2[16]+.5000000000*e[25]*ep2[17]+.5000000000*e[25]*ep2[15]+.5000000000*ep2[10]*e[25]-.5000000000*e[25]*ep2[9]-.5000000000*e[25]*ep2[11]-.5000000000*e[25]*ep2[12]-.5000000000*e[25]*ep2[14];
A[59]=e[19]*e[20]*e[2]+e[22]*e[18]*e[3]+e[22]*e[0]*e[21]+e[22]*e[19]*e[4]+e[22]*e[20]*e[5]+e[22]*e[2]*e[23]-1.*e[19]*e[21]*e[3]-1.*e[19]*e[26]*e[8]+e[19]*e[25]*e[7]-1.*e[19]*e[23]*e[5]-1.*e[19]*e[24]*e[6]+e[4]*e[18]*e[21]+e[4]*e[20]*e[23]+e[25]*e[18]*e[6]+e[25]*e[0]*e[24]+e[25]*e[20]*e[8]+e[25]*e[2]*e[26]+e[7]*e[18]*e[24]+e[7]*e[20]*e[26]+e[19]*e[18]*e[0]+1.500000000*ep2[19]*e[1]+.5000000000*e[1]*ep2[22]+.5000000000*e[1]*ep2[18]+.5000000000*e[1]*ep2[20]+.5000000000*e[1]*ep2[25]-.5000000000*e[1]*ep2[26]-.5000000000*e[1]*ep2[23]-.5000000000*e[1]*ep2[24]-.5000000000*e[1]*ep2[21];
A[169]=e[19]*e[27]*e[24]+e[19]*e[18]*e[33]+e[19]*e[28]*e[25]+e[19]*e[29]*e[26]+e[19]*e[20]*e[35]+e[28]*e[18]*e[24]+e[28]*e[20]*e[26]+e[22]*e[30]*e[24]+e[22]*e[21]*e[33]+e[22]*e[31]*e[25]+e[22]*e[32]*e[26]+e[22]*e[23]*e[35]+e[31]*e[21]*e[24]+e[31]*e[23]*e[26]+e[25]*e[33]*e[24]+e[25]*e[35]*e[26]-1.*e[25]*e[27]*e[18]-1.*e[25]*e[30]*e[21]-1.*e[25]*e[29]*e[20]-1.*e[25]*e[32]*e[23]-.5000000000*e[34]*ep2[18]-.5000000000*e[34]*ep2[23]-.5000000000*e[34]*ep2[20]-.5000000000*e[34]*ep2[21]+.5000000000*ep2[19]*e[34]+.5000000000*ep2[22]*e[34]+1.500000000*e[34]*ep2[25]+.5000000000*e[34]*ep2[24]+.5000000000*e[34]*ep2[26];
A[58]=e[16]*e[0]*e[6]+e[16]*e[2]*e[8]+e[1]*e[11]*e[2]-1.*e[1]*e[15]*e[6]+e[1]*e[9]*e[0]-1.*e[1]*e[14]*e[5]-1.*e[1]*e[12]*e[3]-1.*e[1]*e[17]*e[8]+e[4]*e[9]*e[3]+e[4]*e[0]*e[12]+e[4]*e[1]*e[13]+e[4]*e[11]*e[5]+e[4]*e[2]*e[14]+e[13]*e[0]*e[3]+e[13]*e[2]*e[5]+e[7]*e[9]*e[6]+e[7]*e[0]*e[15]+e[7]*e[1]*e[16]+e[7]*e[11]*e[8]+e[7]*e[2]*e[17]-.5000000000*e[10]*ep2[6]-.5000000000*e[10]*ep2[5]-.5000000000*e[10]*ep2[3]-.5000000000*e[10]*ep2[8]+1.500000000*e[10]*ep2[1]+.5000000000*e[10]*ep2[0]+.5000000000*e[10]*ep2[2]+.5000000000*e[10]*ep2[4]+.5000000000*e[10]*ep2[7];
A[168]=e[13]*e[14]*e[17]+e[13]*e[12]*e[15]+e[10]*e[9]*e[15]+.5000000000*e[16]*ep2[15]-.5000000000*e[16]*ep2[11]-.5000000000*e[16]*ep2[12]-.5000000000*e[16]*ep2[14]+e[10]*e[11]*e[17]+.5000000000*ep2[10]*e[16]+.5000000000*ep3[16]-.5000000000*e[16]*ep2[9]+.5000000000*e[16]*ep2[17]+.5000000000*ep2[13]*e[16];
A[57]=e[10]*e[29]*e[20]+e[22]*e[27]*e[12]+e[22]*e[9]*e[30]+e[22]*e[29]*e[14]+e[22]*e[11]*e[32]+e[22]*e[10]*e[31]+e[31]*e[18]*e[12]+e[31]*e[9]*e[21]+e[31]*e[20]*e[14]+e[31]*e[11]*e[23]-1.*e[10]*e[33]*e[24]-1.*e[10]*e[30]*e[21]-1.*e[10]*e[35]*e[26]-1.*e[10]*e[32]*e[23]+e[10]*e[34]*e[25]+e[19]*e[27]*e[9]+e[19]*e[29]*e[11]+e[28]*e[18]*e[9]+e[28]*e[20]*e[11]+e[16]*e[27]*e[24]+e[16]*e[18]*e[33]+e[16]*e[28]*e[25]+e[16]*e[19]*e[34]+e[16]*e[29]*e[26]+e[16]*e[20]*e[35]-1.*e[19]*e[30]*e[12]-1.*e[19]*e[32]*e[14]-1.*e[19]*e[33]*e[15]-1.*e[19]*e[35]*e[17]-1.*e[28]*e[23]*e[14]-1.*e[28]*e[24]*e[15]-1.*e[28]*e[26]*e[17]-1.*e[28]*e[21]*e[12]+e[25]*e[27]*e[15]+e[25]*e[9]*e[33]+e[25]*e[29]*e[17]+e[25]*e[11]*e[35]+e[34]*e[18]*e[15]+e[34]*e[9]*e[24]+e[34]*e[20]*e[17]+e[34]*e[11]*e[26]+e[13]*e[27]*e[21]+e[13]*e[18]*e[30]+e[13]*e[28]*e[22]+e[13]*e[19]*e[31]+e[13]*e[29]*e[23]+e[13]*e[20]*e[32]+e[10]*e[27]*e[18]+3.*e[10]*e[28]*e[19];
A[171]=e[4]*e[30]*e[15]+e[4]*e[12]*e[33]+e[4]*e[32]*e[17]+e[4]*e[14]*e[35]+e[4]*e[31]*e[16]+e[4]*e[13]*e[34]+e[7]*e[33]*e[15]+e[7]*e[35]*e[17]+3.*e[7]*e[34]*e[16]+e[1]*e[27]*e[15]+e[1]*e[9]*e[33]+e[1]*e[29]*e[17]+e[1]*e[11]*e[35]+e[1]*e[28]*e[16]+e[1]*e[10]*e[34]-1.*e[16]*e[27]*e[0]-1.*e[16]*e[32]*e[5]+e[16]*e[33]*e[6]-1.*e[16]*e[30]*e[3]+e[16]*e[35]*e[8]-1.*e[16]*e[29]*e[2]+e[13]*e[30]*e[6]+e[13]*e[3]*e[33]+e[13]*e[31]*e[7]+e[13]*e[32]*e[8]+e[13]*e[5]*e[35]-1.*e[34]*e[11]*e[2]+e[34]*e[15]*e[6]-1.*e[34]*e[9]*e[0]-1.*e[34]*e[14]*e[5]-1.*e[34]*e[12]*e[3]+e[34]*e[17]*e[8]+e[31]*e[12]*e[6]+e[31]*e[3]*e[15]+e[31]*e[14]*e[8]+e[31]*e[5]*e[17]-1.*e[7]*e[27]*e[9]-1.*e[7]*e[30]*e[12]+e[7]*e[28]*e[10]-1.*e[7]*e[32]*e[14]+e[10]*e[27]*e[6]+e[10]*e[0]*e[33]+e[10]*e[29]*e[8]+e[10]*e[2]*e[35]+e[28]*e[9]*e[6]+e[28]*e[0]*e[15]+e[28]*e[11]*e[8]+e[28]*e[2]*e[17]-1.*e[7]*e[29]*e[11];
A[56]=e[22]*e[18]*e[12]+e[22]*e[9]*e[21]+e[22]*e[20]*e[14]+e[22]*e[11]*e[23]+e[22]*e[19]*e[13]+e[25]*e[18]*e[15]+e[25]*e[9]*e[24]+e[25]*e[20]*e[17]+e[25]*e[11]*e[26]+e[25]*e[19]*e[16]+e[16]*e[18]*e[24]+e[16]*e[20]*e[26]+e[13]*e[18]*e[21]+e[13]*e[20]*e[23]+e[19]*e[18]*e[9]+e[19]*e[20]*e[11]-1.*e[19]*e[23]*e[14]-1.*e[19]*e[24]*e[15]-1.*e[19]*e[26]*e[17]-1.*e[19]*e[21]*e[12]+.5000000000*e[10]*ep2[22]+.5000000000*e[10]*ep2[25]+1.500000000*e[10]*ep2[19]+.5000000000*e[10]*ep2[18]+.5000000000*e[10]*ep2[20]-.5000000000*e[10]*ep2[26]-.5000000000*e[10]*ep2[23]-.5000000000*e[10]*ep2[24]-.5000000000*e[10]*ep2[21];
A[170]=e[19]*e[20]*e[26]-.5000000000*e[25]*ep2[20]+e[22]*e[21]*e[24]+e[19]*e[18]*e[24]+.5000000000*ep2[22]*e[25]-.5000000000*e[25]*ep2[21]-.5000000000*e[25]*ep2[23]+.5000000000*ep2[19]*e[25]-.5000000000*e[25]*ep2[18]+.5000000000*e[25]*ep2[24]+.5000000000*e[25]*ep2[26]+.5000000000*ep3[25]+e[22]*e[23]*e[26];
A[73]=-1.*e[20]*e[33]*e[6]-1.*e[20]*e[30]*e[3]-1.*e[20]*e[31]*e[4]-1.*e[29]*e[21]*e[3]-1.*e[29]*e[22]*e[4]-1.*e[29]*e[25]*e[7]-1.*e[29]*e[24]*e[6]+e[8]*e[27]*e[24]+e[8]*e[18]*e[33]+e[8]*e[28]*e[25]+e[8]*e[19]*e[34]+e[23]*e[27]*e[3]+e[23]*e[0]*e[30]+e[23]*e[28]*e[4]+e[23]*e[1]*e[31]+e[32]*e[18]*e[3]+e[32]*e[0]*e[21]+e[32]*e[19]*e[4]+e[32]*e[1]*e[22]+e[26]*e[27]*e[6]+e[26]*e[0]*e[33]+e[26]*e[28]*e[7]+e[26]*e[1]*e[34]+e[26]*e[29]*e[8]+e[26]*e[2]*e[35]+e[35]*e[18]*e[6]+e[35]*e[0]*e[24]+e[35]*e[19]*e[7]+e[35]*e[1]*e[25]+e[35]*e[20]*e[8]+e[2]*e[27]*e[18]+e[2]*e[28]*e[19]+3.*e[2]*e[29]*e[20]+e[20]*e[27]*e[0]+e[20]*e[28]*e[1]+e[29]*e[18]*e[0]+e[29]*e[19]*e[1]+e[5]*e[27]*e[21]+e[5]*e[18]*e[30]+e[5]*e[28]*e[22]+e[5]*e[19]*e[31]+e[5]*e[29]*e[23]+e[5]*e[20]*e[32]-1.*e[2]*e[33]*e[24]-1.*e[2]*e[30]*e[21]-1.*e[2]*e[31]*e[22]+e[2]*e[32]*e[23]-1.*e[2]*e[34]*e[25]-1.*e[20]*e[34]*e[7];
A[72]=e[5]*e[18]*e[3]+e[5]*e[0]*e[21]+e[5]*e[19]*e[4]+e[5]*e[1]*e[22]+e[5]*e[2]*e[23]+e[23]*e[1]*e[4]+e[23]*e[0]*e[3]+e[8]*e[18]*e[6]+e[8]*e[0]*e[24]+e[8]*e[19]*e[7]+e[8]*e[1]*e[25]+e[8]*e[2]*e[26]+e[26]*e[1]*e[7]+e[26]*e[0]*e[6]+e[2]*e[18]*e[0]+e[2]*e[19]*e[1]-1.*e[2]*e[21]*e[3]-1.*e[2]*e[22]*e[4]-1.*e[2]*e[25]*e[7]-1.*e[2]*e[24]*e[6]-.5000000000*e[20]*ep2[4]+.5000000000*e[20]*ep2[0]-.5000000000*e[20]*ep2[6]+.5000000000*e[20]*ep2[5]+.5000000000*e[20]*ep2[1]-.5000000000*e[20]*ep2[7]-.5000000000*e[20]*ep2[3]+1.500000000*e[20]*ep2[2]+.5000000000*e[20]*ep2[8];
A[75]=e[14]*e[9]*e[3]+e[14]*e[0]*e[12]+e[14]*e[10]*e[4]+e[14]*e[1]*e[13]+e[14]*e[11]*e[5]+e[17]*e[9]*e[6]+e[17]*e[0]*e[15]+e[17]*e[10]*e[7]+e[17]*e[1]*e[16]+e[17]*e[11]*e[8]+e[8]*e[9]*e[15]+e[8]*e[10]*e[16]+e[5]*e[9]*e[12]+e[5]*e[10]*e[13]+e[11]*e[9]*e[0]+e[11]*e[10]*e[1]-1.*e[11]*e[13]*e[4]-1.*e[11]*e[16]*e[7]-1.*e[11]*e[15]*e[6]-1.*e[11]*e[12]*e[3]+.5000000000*e[2]*ep2[14]+.5000000000*e[2]*ep2[17]+1.500000000*e[2]*ep2[11]+.5000000000*e[2]*ep2[9]+.5000000000*e[2]*ep2[10]-.5000000000*e[2]*ep2[16]-.5000000000*e[2]*ep2[12]-.5000000000*e[2]*ep2[15]-.5000000000*e[2]*ep2[13];
A[74]=e[14]*e[18]*e[12]+e[14]*e[9]*e[21]+e[14]*e[11]*e[23]+e[14]*e[19]*e[13]+e[14]*e[10]*e[22]+e[23]*e[9]*e[12]+e[23]*e[10]*e[13]+e[17]*e[18]*e[15]+e[17]*e[9]*e[24]+e[17]*e[11]*e[26]+e[17]*e[19]*e[16]+e[17]*e[10]*e[25]+e[26]*e[9]*e[15]+e[26]*e[10]*e[16]-1.*e[11]*e[24]*e[15]-1.*e[11]*e[25]*e[16]+e[11]*e[18]*e[9]-1.*e[11]*e[21]*e[12]+e[11]*e[19]*e[10]-1.*e[11]*e[22]*e[13]+1.500000000*e[20]*ep2[11]+.5000000000*e[20]*ep2[9]+.5000000000*e[20]*ep2[10]+.5000000000*e[20]*ep2[14]+.5000000000*e[20]*ep2[17]-.5000000000*e[20]*ep2[16]-.5000000000*e[20]*ep2[12]-.5000000000*e[20]*ep2[15]-.5000000000*e[20]*ep2[13];
A[77]=e[23]*e[10]*e[31]+e[32]*e[18]*e[12]+e[32]*e[9]*e[21]+e[32]*e[19]*e[13]+e[32]*e[10]*e[22]-1.*e[11]*e[33]*e[24]-1.*e[11]*e[30]*e[21]+e[11]*e[35]*e[26]-1.*e[11]*e[31]*e[22]-1.*e[11]*e[34]*e[25]+e[20]*e[27]*e[9]+e[20]*e[28]*e[10]+e[29]*e[18]*e[9]+e[29]*e[19]*e[10]+e[17]*e[27]*e[24]+e[17]*e[18]*e[33]+e[17]*e[28]*e[25]+e[17]*e[19]*e[34]+e[17]*e[29]*e[26]+e[17]*e[20]*e[35]-1.*e[20]*e[30]*e[12]-1.*e[20]*e[31]*e[13]-1.*e[20]*e[33]*e[15]-1.*e[20]*e[34]*e[16]-1.*e[29]*e[24]*e[15]-1.*e[29]*e[25]*e[16]-1.*e[29]*e[21]*e[12]-1.*e[29]*e[22]*e[13]+e[26]*e[27]*e[15]+e[26]*e[9]*e[33]+e[26]*e[28]*e[16]+e[26]*e[10]*e[34]+e[35]*e[18]*e[15]+e[35]*e[9]*e[24]+e[35]*e[19]*e[16]+e[35]*e[10]*e[25]+e[14]*e[27]*e[21]+e[14]*e[18]*e[30]+e[14]*e[28]*e[22]+e[14]*e[19]*e[31]+e[14]*e[29]*e[23]+e[14]*e[20]*e[32]+e[11]*e[27]*e[18]+e[11]*e[28]*e[19]+3.*e[11]*e[29]*e[20]+e[23]*e[27]*e[12]+e[23]*e[9]*e[30]+e[23]*e[11]*e[32]+e[23]*e[28]*e[13];
A[76]=e[23]*e[18]*e[12]+e[23]*e[9]*e[21]+e[23]*e[20]*e[14]+e[23]*e[19]*e[13]+e[23]*e[10]*e[22]+e[26]*e[18]*e[15]+e[26]*e[9]*e[24]+e[26]*e[20]*e[17]+e[26]*e[19]*e[16]+e[26]*e[10]*e[25]+e[17]*e[19]*e[25]+e[17]*e[18]*e[24]+e[14]*e[19]*e[22]+e[14]*e[18]*e[21]+e[20]*e[18]*e[9]+e[20]*e[19]*e[10]-1.*e[20]*e[24]*e[15]-1.*e[20]*e[25]*e[16]-1.*e[20]*e[21]*e[12]-1.*e[20]*e[22]*e[13]+.5000000000*e[11]*ep2[23]+.5000000000*e[11]*ep2[26]+.5000000000*e[11]*ep2[19]+.5000000000*e[11]*ep2[18]+1.500000000*e[11]*ep2[20]-.5000000000*e[11]*ep2[22]-.5000000000*e[11]*ep2[24]-.5000000000*e[11]*ep2[21]-.5000000000*e[11]*ep2[25];
A[79]=-1.*e[20]*e[21]*e[3]+e[20]*e[26]*e[8]-1.*e[20]*e[22]*e[4]-1.*e[20]*e[25]*e[7]-1.*e[20]*e[24]*e[6]+e[5]*e[19]*e[22]+e[5]*e[18]*e[21]+e[26]*e[18]*e[6]+e[26]*e[0]*e[24]+e[26]*e[19]*e[7]+e[26]*e[1]*e[25]+e[8]*e[19]*e[25]+e[8]*e[18]*e[24]+e[20]*e[18]*e[0]+e[20]*e[19]*e[1]+e[23]*e[18]*e[3]+e[23]*e[0]*e[21]+e[23]*e[19]*e[4]+e[23]*e[1]*e[22]+e[23]*e[20]*e[5]+1.500000000*ep2[20]*e[2]+.5000000000*e[2]*ep2[23]+.5000000000*e[2]*ep2[19]+.5000000000*e[2]*ep2[18]+.5000000000*e[2]*ep2[26]-.5000000000*e[2]*ep2[22]-.5000000000*e[2]*ep2[24]-.5000000000*e[2]*ep2[21]-.5000000000*e[2]*ep2[25];
A[78]=-1.*e[2]*e[15]*e[6]+e[2]*e[9]*e[0]-1.*e[2]*e[12]*e[3]+e[5]*e[9]*e[3]+e[5]*e[0]*e[12]+e[5]*e[10]*e[4]+e[5]*e[1]*e[13]+e[5]*e[2]*e[14]+e[14]*e[1]*e[4]+e[14]*e[0]*e[3]+e[8]*e[9]*e[6]+e[8]*e[0]*e[15]+e[8]*e[10]*e[7]+e[8]*e[1]*e[16]+e[8]*e[2]*e[17]+e[17]*e[1]*e[7]+e[17]*e[0]*e[6]+e[2]*e[10]*e[1]-1.*e[2]*e[13]*e[4]-1.*e[2]*e[16]*e[7]+.5000000000*e[11]*ep2[1]+.5000000000*e[11]*ep2[0]+1.500000000*e[11]*ep2[2]+.5000000000*e[11]*ep2[5]+.5000000000*e[11]*ep2[8]-.5000000000*e[11]*ep2[4]-.5000000000*e[11]*ep2[6]-.5000000000*e[11]*ep2[7]-.5000000000*e[11]*ep2[3];
A[64]=e[5]*e[19]*e[13]+e[5]*e[10]*e[22]+e[8]*e[18]*e[15]+e[8]*e[9]*e[24]+e[8]*e[20]*e[17]+e[8]*e[11]*e[26]+e[8]*e[19]*e[16]+e[8]*e[10]*e[25]+e[2]*e[18]*e[9]+e[2]*e[19]*e[10]-1.*e[11]*e[21]*e[3]-1.*e[11]*e[22]*e[4]-1.*e[11]*e[25]*e[7]-1.*e[11]*e[24]*e[6]+e[14]*e[18]*e[3]+e[14]*e[0]*e[21]+e[14]*e[19]*e[4]+e[14]*e[1]*e[22]+e[14]*e[2]*e[23]-1.*e[20]*e[13]*e[4]-1.*e[20]*e[16]*e[7]-1.*e[20]*e[15]*e[6]-1.*e[20]*e[12]*e[3]+e[23]*e[9]*e[3]+e[23]*e[0]*e[12]+e[23]*e[10]*e[4]+e[23]*e[1]*e[13]+e[17]*e[18]*e[6]+e[17]*e[0]*e[24]+e[17]*e[19]*e[7]+e[17]*e[1]*e[25]+e[17]*e[2]*e[26]-1.*e[2]*e[24]*e[15]-1.*e[2]*e[25]*e[16]-1.*e[2]*e[21]*e[12]-1.*e[2]*e[22]*e[13]+e[26]*e[9]*e[6]+e[26]*e[0]*e[15]+e[26]*e[10]*e[7]+e[26]*e[1]*e[16]+e[11]*e[18]*e[0]+e[11]*e[19]*e[1]+3.*e[11]*e[20]*e[2]+e[20]*e[9]*e[0]+e[20]*e[10]*e[1]+e[5]*e[18]*e[12]+e[5]*e[9]*e[21]+e[5]*e[20]*e[14]+e[5]*e[11]*e[23];
A[65]=e[32]*e[1]*e[4]+e[32]*e[0]*e[3]+e[8]*e[27]*e[6]+e[8]*e[0]*e[33]+e[8]*e[28]*e[7]+e[8]*e[1]*e[34]+e[35]*e[1]*e[7]+e[35]*e[0]*e[6]+e[2]*e[27]*e[0]+e[2]*e[28]*e[1]-1.*e[2]*e[34]*e[7]+e[2]*e[32]*e[5]-1.*e[2]*e[33]*e[6]-1.*e[2]*e[30]*e[3]+e[2]*e[35]*e[8]-1.*e[2]*e[31]*e[4]+e[5]*e[27]*e[3]+e[5]*e[0]*e[30]+e[5]*e[28]*e[4]+e[5]*e[1]*e[31]+1.500000000*e[29]*ep2[2]-.5000000000*e[29]*ep2[4]+.5000000000*e[29]*ep2[0]-.5000000000*e[29]*ep2[6]+.5000000000*e[29]*ep2[5]+.5000000000*e[29]*ep2[1]-.5000000000*e[29]*ep2[7]-.5000000000*e[29]*ep2[3]+.5000000000*e[29]*ep2[8];
A[66]=e[5]*e[0]*e[3]+e[8]*e[1]*e[7]+e[8]*e[0]*e[6]+e[5]*e[1]*e[4]-.5000000000*e[2]*ep2[4]+.5000000000*ep3[2]+.5000000000*e[2]*ep2[1]-.5000000000*e[2]*ep2[3]+.5000000000*e[2]*ep2[0]+.5000000000*e[2]*ep2[8]+.5000000000*e[2]*ep2[5]-.5000000000*e[2]*ep2[6]-.5000000000*e[2]*ep2[7];
A[67]=e[35]*e[9]*e[15]+e[35]*e[10]*e[16]-1.*e[11]*e[30]*e[12]-1.*e[11]*e[31]*e[13]-1.*e[11]*e[33]*e[15]-1.*e[11]*e[34]*e[16]+e[11]*e[27]*e[9]+e[11]*e[28]*e[10]+e[14]*e[27]*e[12]+e[14]*e[9]*e[30]+e[14]*e[11]*e[32]+e[14]*e[28]*e[13]+e[14]*e[10]*e[31]+e[32]*e[9]*e[12]+e[32]*e[10]*e[13]+e[17]*e[27]*e[15]+e[17]*e[9]*e[33]+e[17]*e[11]*e[35]+e[17]*e[28]*e[16]+e[17]*e[10]*e[34]+1.500000000*e[29]*ep2[11]-.5000000000*e[29]*ep2[16]+.5000000000*e[29]*ep2[9]-.5000000000*e[29]*ep2[12]-.5000000000*e[29]*ep2[15]+.5000000000*e[29]*ep2[17]+.5000000000*e[29]*ep2[10]+.5000000000*e[29]*ep2[14]-.5000000000*e[29]*ep2[13];
A[68]=e[14]*e[9]*e[12]+e[17]*e[10]*e[16]+e[17]*e[9]*e[15]+.5000000000*ep3[11]+e[14]*e[10]*e[13]+.5000000000*e[11]*ep2[10]-.5000000000*e[11]*ep2[15]+.5000000000*e[11]*ep2[14]-.5000000000*e[11]*ep2[13]-.5000000000*e[11]*ep2[12]+.5000000000*e[11]*ep2[9]-.5000000000*e[11]*ep2[16]+.5000000000*e[11]*ep2[17];
A[69]=e[20]*e[27]*e[18]+e[20]*e[28]*e[19]+e[23]*e[27]*e[21]+e[23]*e[18]*e[30]+e[23]*e[28]*e[22]+e[23]*e[19]*e[31]+e[23]*e[20]*e[32]+e[32]*e[19]*e[22]+e[32]*e[18]*e[21]+e[26]*e[27]*e[24]+e[26]*e[18]*e[33]+e[26]*e[28]*e[25]+e[26]*e[19]*e[34]+e[26]*e[20]*e[35]+e[35]*e[19]*e[25]+e[35]*e[18]*e[24]-1.*e[20]*e[33]*e[24]-1.*e[20]*e[30]*e[21]-1.*e[20]*e[31]*e[22]-1.*e[20]*e[34]*e[25]+.5000000000*e[29]*ep2[23]+.5000000000*e[29]*ep2[26]-.5000000000*e[29]*ep2[22]-.5000000000*e[29]*ep2[24]-.5000000000*e[29]*ep2[21]-.5000000000*e[29]*ep2[25]+1.500000000*e[29]*ep2[20]+.5000000000*e[29]*ep2[19]+.5000000000*e[29]*ep2[18];
A[70]=.5000000000*e[20]*ep2[26]+.5000000000*e[20]*ep2[18]+.5000000000*ep3[20]+.5000000000*e[20]*ep2[19]+e[26]*e[18]*e[24]+.5000000000*e[20]*ep2[23]-.5000000000*e[20]*ep2[25]+e[23]*e[19]*e[22]-.5000000000*e[20]*ep2[24]-.5000000000*e[20]*ep2[21]-.5000000000*e[20]*ep2[22]+e[23]*e[18]*e[21]+e[26]*e[19]*e[25];
A[71]=e[8]*e[28]*e[16]+e[8]*e[10]*e[34]+e[2]*e[27]*e[9]+3.*e[2]*e[29]*e[11]+e[2]*e[28]*e[10]+e[11]*e[27]*e[0]-1.*e[11]*e[34]*e[7]-1.*e[11]*e[33]*e[6]-1.*e[11]*e[30]*e[3]+e[11]*e[28]*e[1]-1.*e[11]*e[31]*e[4]+e[14]*e[27]*e[3]+e[14]*e[0]*e[30]+e[14]*e[28]*e[4]+e[14]*e[1]*e[31]+e[14]*e[2]*e[32]+e[29]*e[10]*e[1]-1.*e[29]*e[13]*e[4]-1.*e[29]*e[16]*e[7]-1.*e[29]*e[15]*e[6]+e[29]*e[9]*e[0]-1.*e[29]*e[12]*e[3]+e[32]*e[9]*e[3]+e[32]*e[0]*e[12]+e[32]*e[10]*e[4]+e[32]*e[1]*e[13]+e[17]*e[27]*e[6]+e[17]*e[0]*e[33]+e[17]*e[28]*e[7]+e[17]*e[1]*e[34]+e[17]*e[2]*e[35]-1.*e[2]*e[30]*e[12]-1.*e[2]*e[31]*e[13]-1.*e[2]*e[33]*e[15]-1.*e[2]*e[34]*e[16]+e[35]*e[9]*e[6]+e[35]*e[0]*e[15]+e[35]*e[10]*e[7]+e[35]*e[1]*e[16]+e[5]*e[27]*e[12]+e[5]*e[9]*e[30]+e[5]*e[29]*e[14]+e[5]*e[11]*e[32]+e[5]*e[28]*e[13]+e[5]*e[10]*e[31]+e[8]*e[27]*e[15]+e[8]*e[9]*e[33]+e[8]*e[29]*e[17]+e[8]*e[11]*e[35];
A[91]=-1.*e[12]*e[34]*e[7]+e[12]*e[32]*e[5]-1.*e[12]*e[35]*e[8]-1.*e[12]*e[29]*e[2]-1.*e[12]*e[28]*e[1]+e[12]*e[31]*e[4]-1.*e[30]*e[11]*e[2]-1.*e[30]*e[10]*e[1]+e[30]*e[13]*e[4]-1.*e[30]*e[16]*e[7]+e[30]*e[14]*e[5]-1.*e[30]*e[17]*e[8]+e[15]*e[3]*e[33]+e[15]*e[31]*e[7]+e[15]*e[4]*e[34]+e[15]*e[32]*e[8]+e[15]*e[5]*e[35]+e[3]*e[27]*e[9]-1.*e[3]*e[28]*e[10]-1.*e[3]*e[34]*e[16]-1.*e[3]*e[35]*e[17]-1.*e[3]*e[29]*e[11]+e[33]*e[13]*e[7]+e[33]*e[4]*e[16]+e[33]*e[14]*e[8]+e[33]*e[5]*e[17]+e[9]*e[28]*e[4]+e[9]*e[1]*e[31]+e[9]*e[29]*e[5]+e[9]*e[2]*e[32]+e[27]*e[10]*e[4]+e[27]*e[1]*e[13]+e[27]*e[11]*e[5]+e[27]*e[2]*e[14]+3.*e[3]*e[30]*e[12]+e[3]*e[32]*e[14]+e[3]*e[31]*e[13]+e[6]*e[30]*e[15]+e[6]*e[12]*e[33]+e[6]*e[32]*e[17]+e[6]*e[14]*e[35]+e[6]*e[31]*e[16]+e[6]*e[13]*e[34]+e[0]*e[27]*e[12]+e[0]*e[9]*e[30]+e[0]*e[29]*e[14]+e[0]*e[11]*e[32]+e[0]*e[28]*e[13]+e[0]*e[10]*e[31];
A[90]=.5000000000*e[21]*ep2[24]-.5000000000*e[21]*ep2[25]+.5000000000*e[21]*ep2[23]-.5000000000*e[21]*ep2[26]+.5000000000*ep2[18]*e[21]+.5000000000*e[21]*ep2[22]-.5000000000*e[21]*ep2[20]+e[24]*e[22]*e[25]+e[24]*e[23]*e[26]-.5000000000*e[21]*ep2[19]+e[18]*e[19]*e[22]+e[18]*e[20]*e[23]+.5000000000*ep3[21];
A[89]=-.5000000000*e[30]*ep2[26]-.5000000000*e[30]*ep2[19]-.5000000000*e[30]*ep2[20]-.5000000000*e[30]*ep2[25]+.5000000000*ep2[18]*e[30]+1.500000000*e[30]*ep2[21]+.5000000000*e[30]*ep2[22]+.5000000000*e[30]*ep2[23]+.5000000000*e[30]*ep2[24]+e[18]*e[27]*e[21]+e[18]*e[28]*e[22]+e[18]*e[19]*e[31]+e[18]*e[29]*e[23]+e[18]*e[20]*e[32]+e[27]*e[19]*e[22]+e[27]*e[20]*e[23]+e[21]*e[31]*e[22]+e[21]*e[32]*e[23]+e[24]*e[21]*e[33]+e[24]*e[31]*e[25]+e[24]*e[22]*e[34]+e[24]*e[32]*e[26]+e[24]*e[23]*e[35]+e[33]*e[22]*e[25]+e[33]*e[23]*e[26]-1.*e[21]*e[29]*e[20]-1.*e[21]*e[35]*e[26]-1.*e[21]*e[28]*e[19]-1.*e[21]*e[34]*e[25];
A[88]=.5000000000*e[12]*ep2[15]-.5000000000*e[12]*ep2[17]+e[15]*e[13]*e[16]-.5000000000*e[12]*ep2[10]+e[15]*e[14]*e[17]-.5000000000*e[12]*ep2[16]-.5000000000*e[12]*ep2[11]+e[9]*e[10]*e[13]+.5000000000*e[12]*ep2[13]+.5000000000*ep2[9]*e[12]+.5000000000*ep3[12]+e[9]*e[11]*e[14]+.5000000000*e[12]*ep2[14];
A[95]=e[12]*e[13]*e[4]+e[12]*e[14]*e[5]+e[15]*e[12]*e[6]+e[15]*e[13]*e[7]+e[15]*e[4]*e[16]+e[15]*e[14]*e[8]+e[15]*e[5]*e[17]+e[6]*e[14]*e[17]+e[6]*e[13]*e[16]+e[0]*e[11]*e[14]+e[0]*e[9]*e[12]+e[0]*e[10]*e[13]+e[9]*e[10]*e[4]+e[9]*e[1]*e[13]+e[9]*e[11]*e[5]+e[9]*e[2]*e[14]-1.*e[12]*e[11]*e[2]-1.*e[12]*e[10]*e[1]-1.*e[12]*e[16]*e[7]-1.*e[12]*e[17]*e[8]+1.500000000*ep2[12]*e[3]+.5000000000*e[3]*ep2[15]-.5000000000*e[3]*ep2[16]+.5000000000*e[3]*ep2[9]-.5000000000*e[3]*ep2[11]-.5000000000*e[3]*ep2[17]-.5000000000*e[3]*ep2[10]+.5000000000*e[3]*ep2[14]+.5000000000*e[3]*ep2[13];
A[94]=e[18]*e[11]*e[14]+e[18]*e[9]*e[12]+e[18]*e[10]*e[13]+e[12]*e[23]*e[14]+e[12]*e[22]*e[13]+e[15]*e[12]*e[24]+e[15]*e[23]*e[17]+e[15]*e[14]*e[26]+e[15]*e[22]*e[16]+e[15]*e[13]*e[25]+e[24]*e[14]*e[17]+e[24]*e[13]*e[16]-1.*e[12]*e[25]*e[16]-1.*e[12]*e[26]*e[17]-1.*e[12]*e[20]*e[11]-1.*e[12]*e[19]*e[10]+e[9]*e[20]*e[14]+e[9]*e[11]*e[23]+e[9]*e[19]*e[13]+e[9]*e[10]*e[22]+.5000000000*ep2[9]*e[21]-.5000000000*e[21]*ep2[16]-.5000000000*e[21]*ep2[11]-.5000000000*e[21]*ep2[17]-.5000000000*e[21]*ep2[10]+1.500000000*e[21]*ep2[12]+.5000000000*e[21]*ep2[14]+.5000000000*e[21]*ep2[13]+.5000000000*e[21]*ep2[15];
A[93]=-1.*e[21]*e[35]*e[8]-1.*e[21]*e[29]*e[2]-1.*e[21]*e[28]*e[1]+e[21]*e[31]*e[4]-1.*e[30]*e[26]*e[8]-1.*e[30]*e[20]*e[2]-1.*e[30]*e[19]*e[1]+e[30]*e[22]*e[4]-1.*e[30]*e[25]*e[7]+e[30]*e[23]*e[5]+e[6]*e[31]*e[25]+e[6]*e[22]*e[34]+e[6]*e[32]*e[26]+e[6]*e[23]*e[35]+e[24]*e[30]*e[6]+e[24]*e[3]*e[33]+e[24]*e[31]*e[7]+e[24]*e[4]*e[34]+e[24]*e[32]*e[8]+e[24]*e[5]*e[35]+e[33]*e[21]*e[6]+e[33]*e[22]*e[7]+e[33]*e[4]*e[25]+e[33]*e[23]*e[8]+e[33]*e[5]*e[26]+e[0]*e[27]*e[21]+e[0]*e[18]*e[30]+e[0]*e[28]*e[22]+e[0]*e[19]*e[31]+e[0]*e[29]*e[23]+e[0]*e[20]*e[32]+e[18]*e[27]*e[3]+e[18]*e[28]*e[4]+e[18]*e[1]*e[31]+e[18]*e[29]*e[5]+e[18]*e[2]*e[32]+e[27]*e[19]*e[4]+e[27]*e[1]*e[22]+e[27]*e[20]*e[5]+e[27]*e[2]*e[23]+3.*e[3]*e[30]*e[21]+e[3]*e[31]*e[22]+e[3]*e[32]*e[23]-1.*e[3]*e[29]*e[20]-1.*e[3]*e[35]*e[26]-1.*e[3]*e[28]*e[19]-1.*e[3]*e[34]*e[25]-1.*e[21]*e[34]*e[7]+e[21]*e[32]*e[5];
A[92]=e[18]*e[1]*e[4]+e[18]*e[0]*e[3]+e[18]*e[2]*e[5]+e[3]*e[22]*e[4]+e[3]*e[23]*e[5]+e[6]*e[3]*e[24]+e[6]*e[22]*e[7]+e[6]*e[4]*e[25]+e[6]*e[23]*e[8]+e[6]*e[5]*e[26]+e[24]*e[4]*e[7]+e[24]*e[5]*e[8]+e[0]*e[19]*e[4]+e[0]*e[1]*e[22]+e[0]*e[20]*e[5]+e[0]*e[2]*e[23]-1.*e[3]*e[26]*e[8]-1.*e[3]*e[20]*e[2]-1.*e[3]*e[19]*e[1]-1.*e[3]*e[25]*e[7]+.5000000000*e[21]*ep2[4]+.5000000000*e[21]*ep2[0]+.5000000000*e[21]*ep2[6]+.5000000000*e[21]*ep2[5]-.5000000000*e[21]*ep2[1]-.5000000000*e[21]*ep2[7]+1.500000000*e[21]*ep2[3]-.5000000000*e[21]*ep2[2]-.5000000000*e[21]*ep2[8];
A[82]=.5000000000*ep2[27]*e[21]+1.500000000*e[21]*ep2[30]+.5000000000*e[21]*ep2[32]+.5000000000*e[21]*ep2[31]+.5000000000*e[21]*ep2[33]-.5000000000*e[21]*ep2[28]-.5000000000*e[21]*ep2[29]-.5000000000*e[21]*ep2[34]-.5000000000*e[21]*ep2[35]+e[18]*e[27]*e[30]+e[18]*e[29]*e[32]+e[18]*e[28]*e[31]+e[27]*e[28]*e[22]+e[27]*e[19]*e[31]+e[27]*e[29]*e[23]+e[27]*e[20]*e[32]+e[30]*e[31]*e[22]+e[30]*e[32]*e[23]+e[24]*e[30]*e[33]+e[24]*e[32]*e[35]+e[24]*e[31]*e[34]+e[33]*e[31]*e[25]+e[33]*e[22]*e[34]+e[33]*e[32]*e[26]+e[33]*e[23]*e[35]-1.*e[30]*e[29]*e[20]-1.*e[30]*e[35]*e[26]-1.*e[30]*e[28]*e[19]-1.*e[30]*e[34]*e[25];
A[192]=-.5000000000*e[26]*ep2[4]-.5000000000*e[26]*ep2[0]+.5000000000*e[26]*ep2[6]+.5000000000*e[26]*ep2[5]-.5000000000*e[26]*ep2[1]+.5000000000*e[26]*ep2[7]-.5000000000*e[26]*ep2[3]+.5000000000*e[26]*ep2[2]+1.500000000*e[26]*ep2[8]+e[20]*e[0]*e[6]+e[20]*e[2]*e[8]+e[5]*e[21]*e[6]+e[5]*e[3]*e[24]+e[5]*e[22]*e[7]+e[5]*e[4]*e[25]+e[5]*e[23]*e[8]+e[23]*e[4]*e[7]+e[23]*e[3]*e[6]+e[8]*e[24]*e[6]+e[8]*e[25]*e[7]+e[2]*e[18]*e[6]+e[2]*e[0]*e[24]+e[2]*e[19]*e[7]+e[2]*e[1]*e[25]-1.*e[8]*e[21]*e[3]-1.*e[8]*e[19]*e[1]-1.*e[8]*e[22]*e[4]-1.*e[8]*e[18]*e[0]+e[20]*e[1]*e[7];
A[83]=e[9]*e[27]*e[30]+e[9]*e[29]*e[32]+e[9]*e[28]*e[31]+e[33]*e[30]*e[15]+e[33]*e[32]*e[17]+e[33]*e[14]*e[35]+e[33]*e[31]*e[16]+e[33]*e[13]*e[34]+e[27]*e[29]*e[14]+e[27]*e[11]*e[32]+e[27]*e[28]*e[13]+e[27]*e[10]*e[31]-1.*e[30]*e[28]*e[10]+e[30]*e[31]*e[13]+e[30]*e[32]*e[14]-1.*e[30]*e[34]*e[16]-1.*e[30]*e[35]*e[17]-1.*e[30]*e[29]*e[11]+e[15]*e[32]*e[35]+e[15]*e[31]*e[34]-.5000000000*e[12]*ep2[34]-.5000000000*e[12]*ep2[35]+.5000000000*e[12]*ep2[27]+.5000000000*e[12]*ep2[32]-.5000000000*e[12]*ep2[28]-.5000000000*e[12]*ep2[29]+.5000000000*e[12]*ep2[31]+.5000000000*e[12]*ep2[33]+1.500000000*e[12]*ep2[30];
A[193]=e[23]*e[30]*e[6]+e[23]*e[3]*e[33]+e[23]*e[31]*e[7]+e[23]*e[4]*e[34]+e[32]*e[21]*e[6]+e[32]*e[3]*e[24]+e[32]*e[22]*e[7]+e[32]*e[4]*e[25]+e[26]*e[33]*e[6]+e[26]*e[34]*e[7]+3.*e[26]*e[35]*e[8]+e[35]*e[24]*e[6]+e[35]*e[25]*e[7]+e[2]*e[27]*e[24]+e[2]*e[18]*e[33]+e[2]*e[28]*e[25]+e[2]*e[19]*e[34]+e[2]*e[29]*e[26]+e[2]*e[20]*e[35]+e[20]*e[27]*e[6]+e[20]*e[0]*e[33]+e[20]*e[28]*e[7]+e[20]*e[1]*e[34]+e[20]*e[29]*e[8]+e[29]*e[18]*e[6]+e[29]*e[0]*e[24]+e[29]*e[19]*e[7]+e[29]*e[1]*e[25]+e[5]*e[30]*e[24]+e[5]*e[21]*e[33]+e[5]*e[31]*e[25]+e[5]*e[22]*e[34]+e[5]*e[32]*e[26]+e[5]*e[23]*e[35]-1.*e[8]*e[27]*e[18]+e[8]*e[33]*e[24]-1.*e[8]*e[30]*e[21]-1.*e[8]*e[31]*e[22]+e[8]*e[32]*e[23]-1.*e[8]*e[28]*e[19]+e[8]*e[34]*e[25]-1.*e[26]*e[27]*e[0]-1.*e[26]*e[30]*e[3]-1.*e[26]*e[28]*e[1]-1.*e[26]*e[31]*e[4]-1.*e[35]*e[21]*e[3]-1.*e[35]*e[19]*e[1]-1.*e[35]*e[22]*e[4]-1.*e[35]*e[18]*e[0];
A[80]=e[27]*e[29]*e[32]+e[27]*e[28]*e[31]+e[33]*e[32]*e[35]+e[33]*e[31]*e[34]+.5000000000*ep3[30]-.5000000000*e[30]*ep2[28]-.5000000000*e[30]*ep2[29]-.5000000000*e[30]*ep2[34]+.5000000000*e[30]*ep2[33]+.5000000000*ep2[27]*e[30]+.5000000000*e[30]*ep2[32]+.5000000000*e[30]*ep2[31]-.5000000000*e[30]*ep2[35];
A[194]=.5000000000*ep2[14]*e[26]+1.500000000*e[26]*ep2[17]+.5000000000*e[26]*ep2[15]+.5000000000*e[26]*ep2[16]+.5000000000*ep2[11]*e[26]-.5000000000*e[26]*ep2[9]-.5000000000*e[26]*ep2[12]-.5000000000*e[26]*ep2[10]-.5000000000*e[26]*ep2[13]+e[20]*e[11]*e[17]+e[20]*e[9]*e[15]+e[20]*e[10]*e[16]+e[14]*e[21]*e[15]+e[14]*e[12]*e[24]+e[14]*e[23]*e[17]+e[14]*e[22]*e[16]+e[14]*e[13]*e[25]+e[23]*e[12]*e[15]+e[23]*e[13]*e[16]+e[17]*e[24]*e[15]+e[17]*e[25]*e[16]-1.*e[17]*e[18]*e[9]-1.*e[17]*e[21]*e[12]-1.*e[17]*e[19]*e[10]-1.*e[17]*e[22]*e[13]+e[11]*e[18]*e[15]+e[11]*e[9]*e[24]+e[11]*e[19]*e[16]+e[11]*e[10]*e[25];
A[81]=e[0]*e[27]*e[30]+e[0]*e[29]*e[32]+e[0]*e[28]*e[31]+e[30]*e[31]*e[4]+e[30]*e[32]*e[5]+e[6]*e[30]*e[33]+e[6]*e[32]*e[35]+e[6]*e[31]*e[34]+e[27]*e[28]*e[4]+e[27]*e[1]*e[31]+e[27]*e[29]*e[5]+e[27]*e[2]*e[32]+e[33]*e[31]*e[7]+e[33]*e[4]*e[34]+e[33]*e[32]*e[8]+e[33]*e[5]*e[35]-1.*e[30]*e[34]*e[7]-1.*e[30]*e[35]*e[8]-1.*e[30]*e[29]*e[2]-1.*e[30]*e[28]*e[1]+1.500000000*e[3]*ep2[30]+.5000000000*e[3]*ep2[32]+.5000000000*e[3]*ep2[31]+.5000000000*e[3]*ep2[27]-.5000000000*e[3]*ep2[28]-.5000000000*e[3]*ep2[29]+.5000000000*e[3]*ep2[33]-.5000000000*e[3]*ep2[34]-.5000000000*e[3]*ep2[35];
A[195]=.5000000000*ep2[14]*e[8]+1.500000000*ep2[17]*e[8]+.5000000000*e[8]*ep2[15]+.5000000000*e[8]*ep2[16]-.5000000000*e[8]*ep2[9]+.5000000000*e[8]*ep2[11]-.5000000000*e[8]*ep2[12]-.5000000000*e[8]*ep2[10]-.5000000000*e[8]*ep2[13]+e[14]*e[12]*e[6]+e[14]*e[3]*e[15]+e[14]*e[13]*e[7]+e[14]*e[4]*e[16]+e[14]*e[5]*e[17]+e[17]*e[15]*e[6]+e[17]*e[16]*e[7]+e[2]*e[11]*e[17]+e[2]*e[9]*e[15]+e[2]*e[10]*e[16]+e[5]*e[12]*e[15]+e[5]*e[13]*e[16]+e[11]*e[9]*e[6]+e[11]*e[0]*e[15]+e[11]*e[10]*e[7]+e[11]*e[1]*e[16]-1.*e[17]*e[10]*e[1]-1.*e[17]*e[13]*e[4]-1.*e[17]*e[9]*e[0]-1.*e[17]*e[12]*e[3];
A[86]=-.5000000000*e[3]*ep2[1]-.5000000000*e[3]*ep2[7]+.5000000000*ep3[3]-.5000000000*e[3]*ep2[8]+e[0]*e[2]*e[5]+.5000000000*e[3]*ep2[6]+.5000000000*e[3]*ep2[4]-.5000000000*e[3]*ep2[2]+e[0]*e[1]*e[4]+e[6]*e[4]*e[7]+.5000000000*ep2[0]*e[3]+.5000000000*e[3]*ep2[5]+e[6]*e[5]*e[8];
A[196]=.5000000000*ep2[23]*e[17]+1.500000000*ep2[26]*e[17]+.5000000000*e[17]*ep2[25]+.5000000000*e[17]*ep2[24]-.5000000000*e[17]*ep2[18]-.5000000000*e[17]*ep2[19]+.5000000000*e[17]*ep2[20]-.5000000000*e[17]*ep2[22]-.5000000000*e[17]*ep2[21]+e[23]*e[21]*e[15]+e[23]*e[12]*e[24]+e[23]*e[14]*e[26]+e[23]*e[22]*e[16]+e[23]*e[13]*e[25]+e[26]*e[24]*e[15]+e[26]*e[25]*e[16]+e[11]*e[19]*e[25]+e[11]*e[18]*e[24]+e[11]*e[20]*e[26]+e[14]*e[22]*e[25]+e[14]*e[21]*e[24]+e[20]*e[18]*e[15]+e[20]*e[9]*e[24]+e[20]*e[19]*e[16]+e[20]*e[10]*e[25]-1.*e[26]*e[18]*e[9]-1.*e[26]*e[21]*e[12]-1.*e[26]*e[19]*e[10]-1.*e[26]*e[22]*e[13];
A[87]=-1.*e[12]*e[34]*e[16]-1.*e[12]*e[35]*e[17]-1.*e[12]*e[29]*e[11]+e[9]*e[27]*e[12]+e[9]*e[29]*e[14]+e[9]*e[11]*e[32]+e[9]*e[28]*e[13]+e[9]*e[10]*e[31]+e[27]*e[11]*e[14]+e[27]*e[10]*e[13]+e[12]*e[32]*e[14]+e[12]*e[31]*e[13]+e[15]*e[12]*e[33]+e[15]*e[32]*e[17]+e[15]*e[14]*e[35]+e[15]*e[31]*e[16]+e[15]*e[13]*e[34]+e[33]*e[14]*e[17]+e[33]*e[13]*e[16]-1.*e[12]*e[28]*e[10]+.5000000000*ep2[9]*e[30]-.5000000000*e[30]*ep2[16]-.5000000000*e[30]*ep2[11]+1.500000000*e[30]*ep2[12]+.5000000000*e[30]*ep2[15]-.5000000000*e[30]*ep2[17]-.5000000000*e[30]*ep2[10]+.5000000000*e[30]*ep2[14]+.5000000000*e[30]*ep2[13];
A[197]=e[32]*e[22]*e[16]+e[32]*e[13]*e[25]-1.*e[17]*e[27]*e[18]+e[17]*e[33]*e[24]-1.*e[17]*e[30]*e[21]+e[17]*e[29]*e[20]+3.*e[17]*e[35]*e[26]-1.*e[17]*e[31]*e[22]-1.*e[17]*e[28]*e[19]+e[17]*e[34]*e[25]+e[20]*e[27]*e[15]+e[20]*e[9]*e[33]+e[20]*e[28]*e[16]+e[20]*e[10]*e[34]+e[29]*e[18]*e[15]+e[29]*e[9]*e[24]+e[29]*e[19]*e[16]+e[29]*e[10]*e[25]-1.*e[26]*e[27]*e[9]-1.*e[26]*e[30]*e[12]-1.*e[26]*e[28]*e[10]-1.*e[26]*e[31]*e[13]+e[26]*e[33]*e[15]+e[26]*e[34]*e[16]+e[35]*e[24]*e[15]+e[35]*e[25]*e[16]-1.*e[35]*e[18]*e[9]-1.*e[35]*e[21]*e[12]-1.*e[35]*e[19]*e[10]-1.*e[35]*e[22]*e[13]+e[14]*e[30]*e[24]+e[14]*e[21]*e[33]+e[14]*e[31]*e[25]+e[14]*e[22]*e[34]+e[14]*e[32]*e[26]+e[14]*e[23]*e[35]+e[11]*e[27]*e[24]+e[11]*e[18]*e[33]+e[11]*e[28]*e[25]+e[11]*e[19]*e[34]+e[11]*e[29]*e[26]+e[11]*e[20]*e[35]+e[23]*e[30]*e[15]+e[23]*e[12]*e[33]+e[23]*e[32]*e[17]+e[23]*e[31]*e[16]+e[23]*e[13]*e[34]+e[32]*e[21]*e[15]+e[32]*e[12]*e[24];
A[84]=e[6]*e[23]*e[17]+e[6]*e[14]*e[26]+e[6]*e[22]*e[16]+e[6]*e[13]*e[25]+e[0]*e[20]*e[14]+e[0]*e[11]*e[23]+e[0]*e[19]*e[13]+e[0]*e[10]*e[22]-1.*e[12]*e[26]*e[8]-1.*e[12]*e[20]*e[2]-1.*e[12]*e[19]*e[1]+e[12]*e[22]*e[4]-1.*e[12]*e[25]*e[7]+e[12]*e[23]*e[5]-1.*e[21]*e[11]*e[2]-1.*e[21]*e[10]*e[1]+e[21]*e[13]*e[4]-1.*e[21]*e[16]*e[7]+e[21]*e[14]*e[5]-1.*e[21]*e[17]*e[8]+e[15]*e[3]*e[24]+e[15]*e[22]*e[7]+e[15]*e[4]*e[25]+e[15]*e[23]*e[8]+e[15]*e[5]*e[26]-1.*e[3]*e[25]*e[16]-1.*e[3]*e[26]*e[17]-1.*e[3]*e[20]*e[11]-1.*e[3]*e[19]*e[10]+e[24]*e[13]*e[7]+e[24]*e[4]*e[16]+e[24]*e[14]*e[8]+e[24]*e[5]*e[17]+e[9]*e[18]*e[3]+e[9]*e[0]*e[21]+e[9]*e[19]*e[4]+e[9]*e[1]*e[22]+e[9]*e[20]*e[5]+e[9]*e[2]*e[23]+e[18]*e[0]*e[12]+e[18]*e[10]*e[4]+e[18]*e[1]*e[13]+e[18]*e[11]*e[5]+e[18]*e[2]*e[14]+3.*e[3]*e[21]*e[12]+e[3]*e[23]*e[14]+e[3]*e[22]*e[13]+e[6]*e[21]*e[15]+e[6]*e[12]*e[24];
A[198]=.5000000000*ep2[5]*e[17]+1.500000000*e[17]*ep2[8]+.5000000000*e[17]*ep2[7]+.5000000000*e[17]*ep2[6]+.5000000000*ep2[2]*e[17]-.5000000000*e[17]*ep2[4]-.5000000000*e[17]*ep2[0]-.5000000000*e[17]*ep2[1]-.5000000000*e[17]*ep2[3]+e[11]*e[1]*e[7]+e[11]*e[0]*e[6]+e[11]*e[2]*e[8]+e[5]*e[12]*e[6]+e[5]*e[3]*e[15]+e[5]*e[13]*e[7]+e[5]*e[4]*e[16]+e[5]*e[14]*e[8]+e[14]*e[4]*e[7]+e[14]*e[3]*e[6]+e[8]*e[15]*e[6]+e[8]*e[16]*e[7]-1.*e[8]*e[10]*e[1]-1.*e[8]*e[13]*e[4]-1.*e[8]*e[9]*e[0]-1.*e[8]*e[12]*e[3]+e[2]*e[9]*e[6]+e[2]*e[0]*e[15]+e[2]*e[10]*e[7]+e[2]*e[1]*e[16];
A[85]=e[6]*e[4]*e[34]+e[6]*e[32]*e[8]+e[6]*e[5]*e[35]+e[33]*e[4]*e[7]+e[33]*e[5]*e[8]+e[0]*e[27]*e[3]+e[0]*e[28]*e[4]+e[0]*e[1]*e[31]+e[0]*e[29]*e[5]+e[0]*e[2]*e[32]-1.*e[3]*e[34]*e[7]+e[3]*e[32]*e[5]+e[3]*e[33]*e[6]-1.*e[3]*e[35]*e[8]-1.*e[3]*e[29]*e[2]-1.*e[3]*e[28]*e[1]+e[3]*e[31]*e[4]+e[27]*e[1]*e[4]+e[27]*e[2]*e[5]+e[6]*e[31]*e[7]+.5000000000*e[30]*ep2[4]+.5000000000*e[30]*ep2[6]+.5000000000*e[30]*ep2[5]-.5000000000*e[30]*ep2[1]-.5000000000*e[30]*ep2[7]-.5000000000*e[30]*ep2[2]-.5000000000*e[30]*ep2[8]+.5000000000*ep2[0]*e[30]+1.500000000*e[30]*ep2[3];
A[199]=.5000000000*ep2[23]*e[8]+1.500000000*ep2[26]*e[8]-.5000000000*e[8]*ep2[18]-.5000000000*e[8]*ep2[19]-.5000000000*e[8]*ep2[22]+.5000000000*e[8]*ep2[24]-.5000000000*e[8]*ep2[21]+.5000000000*e[8]*ep2[25]+.5000000000*ep2[20]*e[8]+e[20]*e[18]*e[6]+e[20]*e[0]*e[24]+e[20]*e[19]*e[7]+e[20]*e[1]*e[25]+e[20]*e[2]*e[26]+e[23]*e[21]*e[6]+e[23]*e[3]*e[24]+e[23]*e[22]*e[7]+e[23]*e[4]*e[25]+e[23]*e[5]*e[26]-1.*e[26]*e[21]*e[3]-1.*e[26]*e[19]*e[1]-1.*e[26]*e[22]*e[4]-1.*e[26]*e[18]*e[0]+e[26]*e[25]*e[7]+e[26]*e[24]*e[6]+e[2]*e[19]*e[25]+e[2]*e[18]*e[24]+e[5]*e[22]*e[25]+e[5]*e[21]*e[24];
A[109]=e[19]*e[27]*e[21]+e[19]*e[18]*e[30]+e[19]*e[28]*e[22]+e[19]*e[29]*e[23]+e[19]*e[20]*e[32]+e[28]*e[18]*e[21]+e[28]*e[20]*e[23]+e[22]*e[30]*e[21]+e[22]*e[32]*e[23]+e[25]*e[30]*e[24]+e[25]*e[21]*e[33]+e[25]*e[22]*e[34]+e[25]*e[32]*e[26]+e[25]*e[23]*e[35]+e[34]*e[21]*e[24]+e[34]*e[23]*e[26]-1.*e[22]*e[27]*e[18]-1.*e[22]*e[33]*e[24]-1.*e[22]*e[29]*e[20]-1.*e[22]*e[35]*e[26]+.5000000000*ep2[19]*e[31]+1.500000000*e[31]*ep2[22]+.5000000000*e[31]*ep2[21]+.5000000000*e[31]*ep2[23]+.5000000000*e[31]*ep2[25]-.5000000000*e[31]*ep2[26]-.5000000000*e[31]*ep2[18]-.5000000000*e[31]*ep2[20]-.5000000000*e[31]*ep2[24];
A[108]=-.5000000000*e[13]*ep2[15]+.5000000000*e[13]*ep2[16]+.5000000000*e[13]*ep2[12]+e[16]*e[12]*e[15]+.5000000000*ep3[13]+e[10]*e[11]*e[14]+.5000000000*e[13]*ep2[14]-.5000000000*e[13]*ep2[17]-.5000000000*e[13]*ep2[11]-.5000000000*e[13]*ep2[9]+.5000000000*ep2[10]*e[13]+e[10]*e[9]*e[12]+e[16]*e[14]*e[17];
A[111]=-1.*e[13]*e[29]*e[2]-1.*e[31]*e[11]*e[2]-1.*e[31]*e[15]*e[6]-1.*e[31]*e[9]*e[0]+e[31]*e[14]*e[5]+e[31]*e[12]*e[3]-1.*e[31]*e[17]*e[8]+e[16]*e[30]*e[6]+e[16]*e[3]*e[33]+e[16]*e[4]*e[34]+e[16]*e[32]*e[8]+e[16]*e[5]*e[35]-1.*e[4]*e[27]*e[9]+e[4]*e[28]*e[10]-1.*e[4]*e[33]*e[15]-1.*e[4]*e[35]*e[17]-1.*e[4]*e[29]*e[11]+e[34]*e[12]*e[6]+e[34]*e[3]*e[15]+e[34]*e[14]*e[8]+e[34]*e[5]*e[17]+e[10]*e[27]*e[3]+e[10]*e[0]*e[30]+e[10]*e[29]*e[5]+e[10]*e[2]*e[32]+e[28]*e[9]*e[3]+e[28]*e[0]*e[12]+e[28]*e[11]*e[5]+e[28]*e[2]*e[14]+e[4]*e[30]*e[12]+e[4]*e[32]*e[14]+3.*e[4]*e[31]*e[13]+e[7]*e[30]*e[15]+e[7]*e[12]*e[33]+e[7]*e[32]*e[17]+e[7]*e[14]*e[35]+e[7]*e[31]*e[16]+e[7]*e[13]*e[34]+e[1]*e[27]*e[12]+e[1]*e[9]*e[30]+e[1]*e[29]*e[14]+e[1]*e[11]*e[32]+e[1]*e[28]*e[13]+e[1]*e[10]*e[31]-1.*e[13]*e[27]*e[0]+e[13]*e[32]*e[5]-1.*e[13]*e[33]*e[6]+e[13]*e[30]*e[3]-1.*e[13]*e[35]*e[8];
A[110]=e[25]*e[23]*e[26]+e[19]*e[20]*e[23]+e[19]*e[18]*e[21]+e[25]*e[21]*e[24]+.5000000000*ep3[22]+.5000000000*e[22]*ep2[23]+.5000000000*ep2[19]*e[22]-.5000000000*e[22]*ep2[18]-.5000000000*e[22]*ep2[24]+.5000000000*e[22]*ep2[21]+.5000000000*e[22]*ep2[25]-.5000000000*e[22]*ep2[20]-.5000000000*e[22]*ep2[26];
A[105]=e[34]*e[5]*e[8]+e[1]*e[27]*e[3]+e[1]*e[0]*e[30]+e[1]*e[28]*e[4]+e[1]*e[29]*e[5]+e[1]*e[2]*e[32]-1.*e[4]*e[27]*e[0]+e[4]*e[34]*e[7]+e[4]*e[32]*e[5]-1.*e[4]*e[33]*e[6]+e[4]*e[30]*e[3]-1.*e[4]*e[35]*e[8]-1.*e[4]*e[29]*e[2]+e[28]*e[0]*e[3]+e[28]*e[2]*e[5]+e[7]*e[30]*e[6]+e[7]*e[3]*e[33]+e[7]*e[32]*e[8]+e[7]*e[5]*e[35]+e[34]*e[3]*e[6]+.5000000000*ep2[1]*e[31]+1.500000000*e[31]*ep2[4]-.5000000000*e[31]*ep2[0]-.5000000000*e[31]*ep2[6]+.5000000000*e[31]*ep2[5]+.5000000000*e[31]*ep2[7]+.5000000000*e[31]*ep2[3]-.5000000000*e[31]*ep2[2]-.5000000000*e[31]*ep2[8];
A[104]=e[1]*e[20]*e[14]+e[1]*e[11]*e[23]+e[13]*e[21]*e[3]-1.*e[13]*e[26]*e[8]-1.*e[13]*e[20]*e[2]-1.*e[13]*e[18]*e[0]+e[13]*e[23]*e[5]-1.*e[13]*e[24]*e[6]-1.*e[22]*e[11]*e[2]-1.*e[22]*e[15]*e[6]-1.*e[22]*e[9]*e[0]+e[22]*e[14]*e[5]+e[22]*e[12]*e[3]-1.*e[22]*e[17]*e[8]+e[16]*e[21]*e[6]+e[16]*e[3]*e[24]+e[16]*e[4]*e[25]+e[16]*e[23]*e[8]+e[16]*e[5]*e[26]-1.*e[4]*e[24]*e[15]-1.*e[4]*e[26]*e[17]-1.*e[4]*e[20]*e[11]-1.*e[4]*e[18]*e[9]+e[25]*e[12]*e[6]+e[25]*e[3]*e[15]+e[25]*e[14]*e[8]+e[25]*e[5]*e[17]+e[10]*e[18]*e[3]+e[10]*e[0]*e[21]+e[10]*e[19]*e[4]+e[10]*e[1]*e[22]+e[10]*e[20]*e[5]+e[10]*e[2]*e[23]+e[19]*e[9]*e[3]+e[19]*e[0]*e[12]+e[19]*e[1]*e[13]+e[19]*e[11]*e[5]+e[19]*e[2]*e[14]+e[4]*e[21]*e[12]+e[4]*e[23]*e[14]+3.*e[4]*e[22]*e[13]+e[7]*e[21]*e[15]+e[7]*e[12]*e[24]+e[7]*e[23]*e[17]+e[7]*e[14]*e[26]+e[7]*e[22]*e[16]+e[7]*e[13]*e[25]+e[1]*e[18]*e[12]+e[1]*e[9]*e[21];
A[107]=e[10]*e[27]*e[12]+e[10]*e[9]*e[30]+e[10]*e[29]*e[14]+e[10]*e[11]*e[32]+e[10]*e[28]*e[13]+e[28]*e[11]*e[14]+e[28]*e[9]*e[12]+e[13]*e[30]*e[12]+e[13]*e[32]*e[14]+e[16]*e[30]*e[15]+e[16]*e[12]*e[33]+e[16]*e[32]*e[17]+e[16]*e[14]*e[35]+e[16]*e[13]*e[34]+e[34]*e[14]*e[17]+e[34]*e[12]*e[15]-1.*e[13]*e[27]*e[9]-1.*e[13]*e[33]*e[15]-1.*e[13]*e[35]*e[17]-1.*e[13]*e[29]*e[11]+.5000000000*ep2[10]*e[31]+.5000000000*e[31]*ep2[16]-.5000000000*e[31]*ep2[9]-.5000000000*e[31]*ep2[11]+.5000000000*e[31]*ep2[12]-.5000000000*e[31]*ep2[15]-.5000000000*e[31]*ep2[17]+.5000000000*e[31]*ep2[14]+1.500000000*e[31]*ep2[13];
A[106]=-.5000000000*e[4]*ep2[6]-.5000000000*e[4]*ep2[0]+e[1]*e[2]*e[5]+.5000000000*e[4]*ep2[7]+e[1]*e[0]*e[3]+e[7]*e[5]*e[8]-.5000000000*e[4]*ep2[8]+.5000000000*e[4]*ep2[3]+.5000000000*e[4]*ep2[5]+e[7]*e[3]*e[6]-.5000000000*e[4]*ep2[2]+.5000000000*ep3[4]+.5000000000*ep2[1]*e[4];
A[100]=e[34]*e[32]*e[35]-.5000000000*e[31]*ep2[35]+.5000000000*e[31]*ep2[34]+.5000000000*ep2[28]*e[31]+.5000000000*ep3[31]+.5000000000*e[31]*ep2[32]+e[34]*e[30]*e[33]-.5000000000*e[31]*ep2[27]+.5000000000*e[31]*ep2[30]-.5000000000*e[31]*ep2[33]-.5000000000*e[31]*ep2[29]+e[28]*e[29]*e[32]+e[28]*e[27]*e[30];
A[101]=e[1]*e[27]*e[30]+e[1]*e[29]*e[32]+e[1]*e[28]*e[31]+e[31]*e[30]*e[3]+e[31]*e[32]*e[5]+e[7]*e[30]*e[33]+e[7]*e[32]*e[35]+e[7]*e[31]*e[34]+e[28]*e[27]*e[3]+e[28]*e[0]*e[30]+e[28]*e[29]*e[5]+e[28]*e[2]*e[32]+e[34]*e[30]*e[6]+e[34]*e[3]*e[33]+e[34]*e[32]*e[8]+e[34]*e[5]*e[35]-1.*e[31]*e[27]*e[0]-1.*e[31]*e[33]*e[6]-1.*e[31]*e[35]*e[8]-1.*e[31]*e[29]*e[2]+.5000000000*e[4]*ep2[30]+.5000000000*e[4]*ep2[32]+1.500000000*e[4]*ep2[31]-.5000000000*e[4]*ep2[27]+.5000000000*e[4]*ep2[28]-.5000000000*e[4]*ep2[29]-.5000000000*e[4]*ep2[33]+.5000000000*e[4]*ep2[34]-.5000000000*e[4]*ep2[35];
A[102]=.5000000000*e[22]*ep2[30]+.5000000000*e[22]*ep2[32]+1.500000000*e[22]*ep2[31]+.5000000000*e[22]*ep2[34]-.5000000000*e[22]*ep2[27]-.5000000000*e[22]*ep2[29]-.5000000000*e[22]*ep2[33]-.5000000000*e[22]*ep2[35]+e[28]*e[18]*e[30]+e[28]*e[29]*e[23]+e[28]*e[20]*e[32]+e[31]*e[30]*e[21]+e[31]*e[32]*e[23]+e[25]*e[30]*e[33]+e[25]*e[32]*e[35]+e[25]*e[31]*e[34]+e[34]*e[30]*e[24]+e[34]*e[21]*e[33]+e[34]*e[32]*e[26]+e[34]*e[23]*e[35]-1.*e[31]*e[27]*e[18]-1.*e[31]*e[33]*e[24]-1.*e[31]*e[29]*e[20]-1.*e[31]*e[35]*e[26]+e[19]*e[27]*e[30]+e[19]*e[29]*e[32]+e[19]*e[28]*e[31]+e[28]*e[27]*e[21]+.5000000000*ep2[28]*e[22];
A[103]=e[16]*e[30]*e[33]+e[16]*e[32]*e[35]+e[10]*e[27]*e[30]+e[10]*e[29]*e[32]+e[10]*e[28]*e[31]+e[34]*e[30]*e[15]+e[34]*e[12]*e[33]+e[34]*e[32]*e[17]+e[34]*e[14]*e[35]+e[34]*e[31]*e[16]+e[28]*e[27]*e[12]+e[28]*e[9]*e[30]+e[28]*e[29]*e[14]+e[28]*e[11]*e[32]-1.*e[31]*e[27]*e[9]+e[31]*e[30]*e[12]+e[31]*e[32]*e[14]-1.*e[31]*e[33]*e[15]-1.*e[31]*e[35]*e[17]-1.*e[31]*e[29]*e[11]-.5000000000*e[13]*ep2[27]+.5000000000*e[13]*ep2[32]+.5000000000*e[13]*ep2[28]-.5000000000*e[13]*ep2[29]+1.500000000*e[13]*ep2[31]-.5000000000*e[13]*ep2[33]+.5000000000*e[13]*ep2[30]+.5000000000*e[13]*ep2[34]-.5000000000*e[13]*ep2[35];
A[96]=e[21]*e[23]*e[14]+e[21]*e[22]*e[13]+e[24]*e[21]*e[15]+e[24]*e[23]*e[17]+e[24]*e[14]*e[26]+e[24]*e[22]*e[16]+e[24]*e[13]*e[25]+e[15]*e[22]*e[25]+e[15]*e[23]*e[26]+e[9]*e[19]*e[22]+e[9]*e[18]*e[21]+e[9]*e[20]*e[23]+e[18]*e[20]*e[14]+e[18]*e[11]*e[23]+e[18]*e[19]*e[13]+e[18]*e[10]*e[22]-1.*e[21]*e[25]*e[16]-1.*e[21]*e[26]*e[17]-1.*e[21]*e[20]*e[11]-1.*e[21]*e[19]*e[10]+1.500000000*ep2[21]*e[12]+.5000000000*e[12]*ep2[24]-.5000000000*e[12]*ep2[26]+.5000000000*e[12]*ep2[18]+.5000000000*e[12]*ep2[23]-.5000000000*e[12]*ep2[19]-.5000000000*e[12]*ep2[20]+.5000000000*e[12]*ep2[22]-.5000000000*e[12]*ep2[25];
A[97]=-1.*e[12]*e[29]*e[20]-1.*e[12]*e[35]*e[26]-1.*e[12]*e[28]*e[19]-1.*e[12]*e[34]*e[25]+e[18]*e[29]*e[14]+e[18]*e[11]*e[32]+e[18]*e[28]*e[13]+e[18]*e[10]*e[31]+e[27]*e[20]*e[14]+e[27]*e[11]*e[23]+e[27]*e[19]*e[13]+e[27]*e[10]*e[22]+e[15]*e[30]*e[24]+e[15]*e[21]*e[33]+e[15]*e[31]*e[25]+e[15]*e[22]*e[34]+e[15]*e[32]*e[26]+e[15]*e[23]*e[35]-1.*e[21]*e[28]*e[10]-1.*e[21]*e[34]*e[16]-1.*e[21]*e[35]*e[17]-1.*e[21]*e[29]*e[11]-1.*e[30]*e[25]*e[16]-1.*e[30]*e[26]*e[17]-1.*e[30]*e[20]*e[11]-1.*e[30]*e[19]*e[10]+e[24]*e[32]*e[17]+e[24]*e[14]*e[35]+e[24]*e[31]*e[16]+e[24]*e[13]*e[34]+e[33]*e[23]*e[17]+e[33]*e[14]*e[26]+e[33]*e[22]*e[16]+e[33]*e[13]*e[25]+3.*e[12]*e[30]*e[21]+e[12]*e[31]*e[22]+e[12]*e[32]*e[23]+e[9]*e[27]*e[21]+e[9]*e[18]*e[30]+e[9]*e[28]*e[22]+e[9]*e[19]*e[31]+e[9]*e[29]*e[23]+e[9]*e[20]*e[32]+e[21]*e[32]*e[14]+e[21]*e[31]*e[13]+e[30]*e[23]*e[14]+e[30]*e[22]*e[13]+e[12]*e[27]*e[18]+e[12]*e[33]*e[24];
A[98]=e[0]*e[11]*e[5]+e[0]*e[2]*e[14]+e[9]*e[1]*e[4]+e[9]*e[0]*e[3]+e[9]*e[2]*e[5]+e[3]*e[13]*e[4]+e[3]*e[14]*e[5]+e[6]*e[3]*e[15]+e[6]*e[13]*e[7]+e[6]*e[4]*e[16]+e[6]*e[14]*e[8]+e[6]*e[5]*e[17]+e[15]*e[4]*e[7]+e[15]*e[5]*e[8]-1.*e[3]*e[11]*e[2]-1.*e[3]*e[10]*e[1]-1.*e[3]*e[16]*e[7]-1.*e[3]*e[17]*e[8]+e[0]*e[10]*e[4]+e[0]*e[1]*e[13]+1.500000000*e[12]*ep2[3]+.5000000000*e[12]*ep2[4]+.5000000000*e[12]*ep2[5]+.5000000000*e[12]*ep2[6]+.5000000000*ep2[0]*e[12]-.5000000000*e[12]*ep2[1]-.5000000000*e[12]*ep2[7]-.5000000000*e[12]*ep2[2]-.5000000000*e[12]*ep2[8];
A[99]=e[21]*e[24]*e[6]+e[0]*e[19]*e[22]+e[0]*e[20]*e[23]+e[24]*e[22]*e[7]+e[24]*e[4]*e[25]+e[24]*e[23]*e[8]+e[24]*e[5]*e[26]+e[6]*e[22]*e[25]+e[6]*e[23]*e[26]+e[18]*e[0]*e[21]+e[18]*e[19]*e[4]+e[18]*e[1]*e[22]+e[18]*e[20]*e[5]+e[18]*e[2]*e[23]+e[21]*e[22]*e[4]+e[21]*e[23]*e[5]-1.*e[21]*e[26]*e[8]-1.*e[21]*e[20]*e[2]-1.*e[21]*e[19]*e[1]-1.*e[21]*e[25]*e[7]+1.500000000*ep2[21]*e[3]+.5000000000*e[3]*ep2[22]+.5000000000*e[3]*ep2[23]+.5000000000*e[3]*ep2[24]-.5000000000*e[3]*ep2[26]-.5000000000*e[3]*ep2[19]-.5000000000*e[3]*ep2[20]-.5000000000*e[3]*ep2[25]+.5000000000*ep2[18]*e[3];
A[127]=e[11]*e[27]*e[12]+e[11]*e[9]*e[30]+e[11]*e[29]*e[14]+e[11]*e[28]*e[13]+e[11]*e[10]*e[31]+e[29]*e[9]*e[12]+e[29]*e[10]*e[13]+e[14]*e[30]*e[12]+e[14]*e[31]*e[13]+e[17]*e[30]*e[15]+e[17]*e[12]*e[33]+e[17]*e[14]*e[35]+e[17]*e[31]*e[16]+e[17]*e[13]*e[34]+e[35]*e[12]*e[15]+e[35]*e[13]*e[16]-1.*e[14]*e[27]*e[9]-1.*e[14]*e[28]*e[10]-1.*e[14]*e[33]*e[15]-1.*e[14]*e[34]*e[16]+.5000000000*ep2[11]*e[32]-.5000000000*e[32]*ep2[16]-.5000000000*e[32]*ep2[9]+.5000000000*e[32]*ep2[12]-.5000000000*e[32]*ep2[15]+.5000000000*e[32]*ep2[17]-.5000000000*e[32]*ep2[10]+1.500000000*e[32]*ep2[14]+.5000000000*e[32]*ep2[13];
A[126]=e[8]*e[3]*e[6]+.5000000000*ep2[2]*e[5]-.5000000000*e[5]*ep2[0]+.5000000000*e[5]*ep2[4]-.5000000000*e[5]*ep2[6]+.5000000000*e[5]*ep2[8]+e[8]*e[4]*e[7]+.5000000000*ep3[5]+e[2]*e[0]*e[3]+.5000000000*e[5]*ep2[3]-.5000000000*e[5]*ep2[7]+e[2]*e[1]*e[4]-.5000000000*e[5]*ep2[1];
A[125]=e[2]*e[27]*e[3]+e[2]*e[0]*e[30]+e[2]*e[28]*e[4]+e[2]*e[1]*e[31]+e[2]*e[29]*e[5]-1.*e[5]*e[27]*e[0]-1.*e[5]*e[34]*e[7]-1.*e[5]*e[33]*e[6]+e[5]*e[30]*e[3]+e[5]*e[35]*e[8]-1.*e[5]*e[28]*e[1]+e[5]*e[31]*e[4]+e[29]*e[1]*e[4]+e[29]*e[0]*e[3]+e[8]*e[30]*e[6]+e[8]*e[3]*e[33]+e[8]*e[31]*e[7]+e[8]*e[4]*e[34]+e[35]*e[4]*e[7]+e[35]*e[3]*e[6]+.5000000000*ep2[2]*e[32]+1.500000000*e[32]*ep2[5]+.5000000000*e[32]*ep2[4]-.5000000000*e[32]*ep2[0]-.5000000000*e[32]*ep2[6]-.5000000000*e[32]*ep2[1]-.5000000000*e[32]*ep2[7]+.5000000000*e[32]*ep2[3]+.5000000000*e[32]*ep2[8];
A[124]=-1.*e[14]*e[19]*e[1]+e[14]*e[22]*e[4]-1.*e[14]*e[18]*e[0]-1.*e[14]*e[25]*e[7]-1.*e[14]*e[24]*e[6]-1.*e[23]*e[10]*e[1]+e[23]*e[13]*e[4]-1.*e[23]*e[16]*e[7]-1.*e[23]*e[15]*e[6]-1.*e[23]*e[9]*e[0]+e[23]*e[12]*e[3]+e[17]*e[21]*e[6]+e[17]*e[3]*e[24]+e[17]*e[22]*e[7]+e[17]*e[4]*e[25]+e[17]*e[5]*e[26]-1.*e[5]*e[24]*e[15]-1.*e[5]*e[25]*e[16]-1.*e[5]*e[18]*e[9]-1.*e[5]*e[19]*e[10]+e[26]*e[12]*e[6]+e[26]*e[3]*e[15]+e[26]*e[13]*e[7]+e[26]*e[4]*e[16]+e[11]*e[18]*e[3]+e[11]*e[0]*e[21]+e[11]*e[19]*e[4]+e[11]*e[1]*e[22]+e[11]*e[20]*e[5]+e[11]*e[2]*e[23]+e[20]*e[9]*e[3]+e[20]*e[0]*e[12]+e[20]*e[10]*e[4]+e[20]*e[1]*e[13]+e[20]*e[2]*e[14]+e[5]*e[21]*e[12]+3.*e[5]*e[23]*e[14]+e[5]*e[22]*e[13]+e[8]*e[21]*e[15]+e[8]*e[12]*e[24]+e[8]*e[23]*e[17]+e[8]*e[14]*e[26]+e[8]*e[22]*e[16]+e[8]*e[13]*e[25]+e[2]*e[18]*e[12]+e[2]*e[9]*e[21]+e[2]*e[19]*e[13]+e[2]*e[10]*e[22]+e[14]*e[21]*e[3];
A[123]=-.5000000000*e[14]*ep2[27]+1.500000000*e[14]*ep2[32]-.5000000000*e[14]*ep2[28]+.5000000000*e[14]*ep2[29]+.5000000000*e[14]*ep2[31]-.5000000000*e[14]*ep2[33]+.5000000000*e[14]*ep2[30]-.5000000000*e[14]*ep2[34]+.5000000000*e[14]*ep2[35]+e[11]*e[27]*e[30]+e[11]*e[29]*e[32]+e[11]*e[28]*e[31]+e[35]*e[30]*e[15]+e[35]*e[12]*e[33]+e[35]*e[32]*e[17]+e[35]*e[31]*e[16]+e[35]*e[13]*e[34]+e[29]*e[27]*e[12]+e[29]*e[9]*e[30]+e[29]*e[28]*e[13]+e[29]*e[10]*e[31]-1.*e[32]*e[27]*e[9]+e[32]*e[30]*e[12]-1.*e[32]*e[28]*e[10]+e[32]*e[31]*e[13]-1.*e[32]*e[33]*e[15]-1.*e[32]*e[34]*e[16]+e[17]*e[30]*e[33]+e[17]*e[31]*e[34];
A[122]=-.5000000000*e[23]*ep2[33]-.5000000000*e[23]*ep2[34]+.5000000000*ep2[29]*e[23]+.5000000000*e[23]*ep2[30]+1.500000000*e[23]*ep2[32]+.5000000000*e[23]*ep2[31]+.5000000000*e[23]*ep2[35]-.5000000000*e[23]*ep2[27]-.5000000000*e[23]*ep2[28]+e[32]*e[30]*e[21]+e[32]*e[31]*e[22]+e[26]*e[30]*e[33]+e[26]*e[32]*e[35]+e[26]*e[31]*e[34]+e[35]*e[30]*e[24]+e[35]*e[21]*e[33]+e[35]*e[31]*e[25]+e[35]*e[22]*e[34]-1.*e[32]*e[27]*e[18]-1.*e[32]*e[33]*e[24]-1.*e[32]*e[28]*e[19]-1.*e[32]*e[34]*e[25]+e[20]*e[27]*e[30]+e[20]*e[29]*e[32]+e[20]*e[28]*e[31]+e[29]*e[27]*e[21]+e[29]*e[18]*e[30]+e[29]*e[28]*e[22]+e[29]*e[19]*e[31];
A[121]=e[2]*e[27]*e[30]+e[2]*e[29]*e[32]+e[2]*e[28]*e[31]+e[32]*e[30]*e[3]+e[32]*e[31]*e[4]+e[8]*e[30]*e[33]+e[8]*e[32]*e[35]+e[8]*e[31]*e[34]+e[29]*e[27]*e[3]+e[29]*e[0]*e[30]+e[29]*e[28]*e[4]+e[29]*e[1]*e[31]+e[35]*e[30]*e[6]+e[35]*e[3]*e[33]+e[35]*e[31]*e[7]+e[35]*e[4]*e[34]-1.*e[32]*e[27]*e[0]-1.*e[32]*e[34]*e[7]-1.*e[32]*e[33]*e[6]-1.*e[32]*e[28]*e[1]+.5000000000*e[5]*ep2[30]+1.500000000*e[5]*ep2[32]+.5000000000*e[5]*ep2[31]-.5000000000*e[5]*ep2[27]-.5000000000*e[5]*ep2[28]+.5000000000*e[5]*ep2[29]-.5000000000*e[5]*ep2[33]-.5000000000*e[5]*ep2[34]+.5000000000*e[5]*ep2[35];
A[120]=.5000000000*e[32]*ep2[31]+.5000000000*e[32]*ep2[35]-.5000000000*e[32]*ep2[27]+e[29]*e[27]*e[30]+e[29]*e[28]*e[31]+e[35]*e[30]*e[33]+e[35]*e[31]*e[34]+.5000000000*ep2[29]*e[32]+.5000000000*ep3[32]-.5000000000*e[32]*ep2[33]-.5000000000*e[32]*ep2[34]+.5000000000*e[32]*ep2[30]-.5000000000*e[32]*ep2[28];
A[118]=e[10]*e[1]*e[4]+e[10]*e[0]*e[3]+e[10]*e[2]*e[5]+e[4]*e[12]*e[3]+e[4]*e[14]*e[5]+e[7]*e[12]*e[6]+e[7]*e[3]*e[15]+e[7]*e[4]*e[16]+e[7]*e[14]*e[8]+e[7]*e[5]*e[17]+e[16]*e[3]*e[6]+e[16]*e[5]*e[8]-1.*e[4]*e[11]*e[2]-1.*e[4]*e[15]*e[6]-1.*e[4]*e[9]*e[0]-1.*e[4]*e[17]*e[8]+e[1]*e[9]*e[3]+e[1]*e[0]*e[12]+e[1]*e[11]*e[5]+e[1]*e[2]*e[14]+1.500000000*e[13]*ep2[4]+.5000000000*e[13]*ep2[3]+.5000000000*e[13]*ep2[5]+.5000000000*e[13]*ep2[7]+.5000000000*ep2[1]*e[13]-.5000000000*e[13]*ep2[0]-.5000000000*e[13]*ep2[6]-.5000000000*e[13]*ep2[2]-.5000000000*e[13]*ep2[8];
A[119]=e[25]*e[21]*e[6]+e[25]*e[3]*e[24]+e[25]*e[23]*e[8]+e[25]*e[5]*e[26]+e[7]*e[21]*e[24]+e[7]*e[23]*e[26]+e[19]*e[18]*e[3]+e[19]*e[0]*e[21]+e[19]*e[1]*e[22]+e[19]*e[20]*e[5]+e[19]*e[2]*e[23]+e[22]*e[21]*e[3]+e[22]*e[23]*e[5]-1.*e[22]*e[26]*e[8]-1.*e[22]*e[20]*e[2]-1.*e[22]*e[18]*e[0]+e[22]*e[25]*e[7]-1.*e[22]*e[24]*e[6]+e[1]*e[18]*e[21]+e[1]*e[20]*e[23]+.5000000000*e[4]*ep2[25]-.5000000000*e[4]*ep2[26]-.5000000000*e[4]*ep2[18]-.5000000000*e[4]*ep2[20]-.5000000000*e[4]*ep2[24]+.5000000000*ep2[19]*e[4]+1.500000000*ep2[22]*e[4]+.5000000000*e[4]*ep2[21]+.5000000000*e[4]*ep2[23];
A[116]=e[22]*e[21]*e[12]+e[22]*e[23]*e[14]+e[25]*e[21]*e[15]+e[25]*e[12]*e[24]+e[25]*e[23]*e[17]+e[25]*e[14]*e[26]+e[25]*e[22]*e[16]+e[16]*e[21]*e[24]+e[16]*e[23]*e[26]+e[10]*e[19]*e[22]+e[10]*e[18]*e[21]+e[10]*e[20]*e[23]+e[19]*e[18]*e[12]+e[19]*e[9]*e[21]+e[19]*e[20]*e[14]+e[19]*e[11]*e[23]-1.*e[22]*e[24]*e[15]-1.*e[22]*e[26]*e[17]-1.*e[22]*e[20]*e[11]-1.*e[22]*e[18]*e[9]-.5000000000*e[13]*ep2[26]-.5000000000*e[13]*ep2[18]+.5000000000*e[13]*ep2[23]+.5000000000*e[13]*ep2[19]-.5000000000*e[13]*ep2[20]-.5000000000*e[13]*ep2[24]+.5000000000*e[13]*ep2[21]+1.500000000*ep2[22]*e[13]+.5000000000*e[13]*ep2[25];
A[117]=e[13]*e[30]*e[21]+3.*e[13]*e[31]*e[22]+e[13]*e[32]*e[23]+e[10]*e[27]*e[21]+e[10]*e[18]*e[30]+e[10]*e[28]*e[22]+e[10]*e[19]*e[31]+e[10]*e[29]*e[23]+e[10]*e[20]*e[32]+e[22]*e[30]*e[12]+e[22]*e[32]*e[14]+e[31]*e[21]*e[12]+e[31]*e[23]*e[14]-1.*e[13]*e[27]*e[18]-1.*e[13]*e[33]*e[24]-1.*e[13]*e[29]*e[20]-1.*e[13]*e[35]*e[26]+e[13]*e[28]*e[19]+e[13]*e[34]*e[25]+e[19]*e[27]*e[12]+e[19]*e[9]*e[30]+e[19]*e[29]*e[14]+e[19]*e[11]*e[32]+e[28]*e[18]*e[12]+e[28]*e[9]*e[21]+e[28]*e[20]*e[14]+e[28]*e[11]*e[23]+e[16]*e[30]*e[24]+e[16]*e[21]*e[33]+e[16]*e[31]*e[25]+e[16]*e[22]*e[34]+e[16]*e[32]*e[26]+e[16]*e[23]*e[35]-1.*e[22]*e[27]*e[9]-1.*e[22]*e[33]*e[15]-1.*e[22]*e[35]*e[17]-1.*e[22]*e[29]*e[11]-1.*e[31]*e[24]*e[15]-1.*e[31]*e[26]*e[17]-1.*e[31]*e[20]*e[11]-1.*e[31]*e[18]*e[9]+e[25]*e[30]*e[15]+e[25]*e[12]*e[33]+e[25]*e[32]*e[17]+e[25]*e[14]*e[35]+e[34]*e[21]*e[15]+e[34]*e[12]*e[24]+e[34]*e[23]*e[17]+e[34]*e[14]*e[26];
A[114]=e[19]*e[11]*e[14]+e[19]*e[9]*e[12]+e[19]*e[10]*e[13]+e[13]*e[21]*e[12]+e[13]*e[23]*e[14]+e[16]*e[21]*e[15]+e[16]*e[12]*e[24]+e[16]*e[23]*e[17]+e[16]*e[14]*e[26]+e[16]*e[13]*e[25]+e[25]*e[14]*e[17]+e[25]*e[12]*e[15]-1.*e[13]*e[24]*e[15]-1.*e[13]*e[26]*e[17]-1.*e[13]*e[20]*e[11]-1.*e[13]*e[18]*e[9]+e[10]*e[18]*e[12]+e[10]*e[9]*e[21]+e[10]*e[20]*e[14]+e[10]*e[11]*e[23]+1.500000000*e[22]*ep2[13]+.5000000000*e[22]*ep2[14]+.5000000000*e[22]*ep2[12]+.5000000000*e[22]*ep2[16]+.5000000000*ep2[10]*e[22]-.5000000000*e[22]*ep2[9]-.5000000000*e[22]*ep2[11]-.5000000000*e[22]*ep2[15]-.5000000000*e[22]*ep2[17];
A[115]=e[13]*e[12]*e[3]+e[13]*e[14]*e[5]+e[16]*e[12]*e[6]+e[16]*e[3]*e[15]+e[16]*e[13]*e[7]+e[16]*e[14]*e[8]+e[16]*e[5]*e[17]+e[7]*e[14]*e[17]+e[7]*e[12]*e[15]+e[1]*e[11]*e[14]+e[1]*e[9]*e[12]+e[1]*e[10]*e[13]+e[10]*e[9]*e[3]+e[10]*e[0]*e[12]+e[10]*e[11]*e[5]+e[10]*e[2]*e[14]-1.*e[13]*e[11]*e[2]-1.*e[13]*e[15]*e[6]-1.*e[13]*e[9]*e[0]-1.*e[13]*e[17]*e[8]+1.500000000*ep2[13]*e[4]+.5000000000*e[4]*ep2[16]-.5000000000*e[4]*ep2[9]-.5000000000*e[4]*ep2[11]+.5000000000*e[4]*ep2[12]-.5000000000*e[4]*ep2[15]-.5000000000*e[4]*ep2[17]+.5000000000*e[4]*ep2[10]+.5000000000*e[4]*ep2[14];
A[112]=e[19]*e[1]*e[4]+e[19]*e[0]*e[3]+e[19]*e[2]*e[5]+e[4]*e[21]*e[3]+e[4]*e[23]*e[5]+e[7]*e[21]*e[6]+e[7]*e[3]*e[24]+e[7]*e[4]*e[25]+e[7]*e[23]*e[8]+e[7]*e[5]*e[26]+e[25]*e[3]*e[6]+e[25]*e[5]*e[8]+e[1]*e[18]*e[3]+e[1]*e[0]*e[21]+e[1]*e[20]*e[5]+e[1]*e[2]*e[23]-1.*e[4]*e[26]*e[8]-1.*e[4]*e[20]*e[2]-1.*e[4]*e[18]*e[0]-1.*e[4]*e[24]*e[6]+1.500000000*e[22]*ep2[4]-.5000000000*e[22]*ep2[0]-.5000000000*e[22]*ep2[6]+.5000000000*e[22]*ep2[5]+.5000000000*e[22]*ep2[1]+.5000000000*e[22]*ep2[7]+.5000000000*e[22]*ep2[3]-.5000000000*e[22]*ep2[2]-.5000000000*e[22]*ep2[8];
A[113]=-1.*e[31]*e[20]*e[2]-1.*e[31]*e[18]*e[0]+e[31]*e[23]*e[5]-1.*e[31]*e[24]*e[6]+e[7]*e[30]*e[24]+e[7]*e[21]*e[33]+e[7]*e[32]*e[26]+e[7]*e[23]*e[35]+e[25]*e[30]*e[6]+e[25]*e[3]*e[33]+e[25]*e[31]*e[7]+e[25]*e[4]*e[34]+e[25]*e[32]*e[8]+e[25]*e[5]*e[35]+e[34]*e[21]*e[6]+e[34]*e[3]*e[24]+e[34]*e[22]*e[7]+e[34]*e[23]*e[8]+e[34]*e[5]*e[26]+e[1]*e[27]*e[21]+e[1]*e[18]*e[30]+e[1]*e[28]*e[22]+e[1]*e[19]*e[31]+e[1]*e[29]*e[23]+e[1]*e[20]*e[32]+e[19]*e[27]*e[3]+e[19]*e[0]*e[30]+e[19]*e[28]*e[4]+e[19]*e[29]*e[5]+e[19]*e[2]*e[32]+e[28]*e[18]*e[3]+e[28]*e[0]*e[21]+e[28]*e[20]*e[5]+e[28]*e[2]*e[23]+e[4]*e[30]*e[21]+3.*e[4]*e[31]*e[22]+e[4]*e[32]*e[23]-1.*e[4]*e[27]*e[18]-1.*e[4]*e[33]*e[24]-1.*e[4]*e[29]*e[20]-1.*e[4]*e[35]*e[26]-1.*e[22]*e[27]*e[0]+e[22]*e[32]*e[5]-1.*e[22]*e[33]*e[6]+e[22]*e[30]*e[3]-1.*e[22]*e[35]*e[8]-1.*e[22]*e[29]*e[2]+e[31]*e[21]*e[3]-1.*e[31]*e[26]*e[8];
int perm[20] = {6, 8, 18, 15, 12, 5, 14, 7, 4, 11, 19, 13, 1, 16, 17, 3, 10, 9, 2, 0};
double AA[200];
for (int i = 0; i < 20; i++)
{
for (int j = 0; j < 10; j++) AA[i + j * 20] = A[perm[i] + j * 20];
}
for (int i = 0; i < 200; i++)
{
A[i] = AA[i];
}
}
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
{
Mat X1 = _m1.getMat(), X2 = _m2.getMat(), model = _model.getMat();
const Point2d* x1ptr = X1.ptr<Point2d>();
const Point2d* x2ptr = X2.ptr<Point2d>();
int n = X1.checkVector(2);
Matx33d E(model.ptr<double>());
_err.create(n, 1, CV_32F);
Mat err = _err.getMat();
for (int i = 0; i < n; i++)
{
Vec3d x1(x1ptr[i].x, x1ptr[i].y, 1.);
Vec3d x2(x2ptr[i].x, x2ptr[i].y, 1.);
Vec3d Ex1 = E * x1;
Vec3d Etx2 = E.t() * x2;
double x2tEx1 = x2.dot(Ex1);
double a = Ex1[0] * Ex1[0];
double b = Ex1[1] * Ex1[1];
double c = Etx2[0] * Etx2[0];
double d = Etx2[1] * Etx2[1];
err.at<float>(i) = (float)(x2tEx1 * x2tEx1 / (a + b + c + d));
}
}
};
}
// Input should be a vector of n 2D points or a Nx2 matrix
cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
int method, double prob, double threshold, OutputArray _mask)
{
CV_INSTRUMENT_REGION();
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
_cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
int npoints = points1.checkVector(2);
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type());
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
if (points1.channels() > 1)
{
points1 = points1.reshape(1, npoints);
points2 = points2.reshape(1, npoints);
}
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double cx = cameraMatrix.at<double>(0,2);
double cy = cameraMatrix.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx) / fx;
points2.col(0) = (points2.col(0) - cx) / fx;
points1.col(1) = (points1.col(1) - cy) / fy;
points2.col(1) = (points2.col(1) - cy) / fy;
// Reshape data to fit opencv ransac function
points1 = points1.reshape(2, npoints);
points2 = points2.reshape(2, npoints);
threshold /= (fx+fy)/2;
Mat E;
if( method == RANSAC )
createRANSACPointSetRegistrator(makePtr<EMEstimatorCallback>(), 5, threshold, prob)->run(points1, points2, E, _mask);
else
createLMeDSPointSetRegistrator(makePtr<EMEstimatorCallback>(), 5, prob)->run(points1, points2, E, _mask);
return E;
}
cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, double focal, Point2d pp,
int method, double prob, double threshold, OutputArray _mask)
{
CV_INSTRUMENT_REGION();
Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);
return cv::findEssentialMat(_points1, _points2, cameraMatrix, method, prob, threshold, _mask);
}
int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2,
InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
InputOutputArray _mask, OutputArray triangulatedPoints)
{
CV_INSTRUMENT_REGION();
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
_cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
int npoints = points1.checkVector(2);
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type());
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
if (points1.channels() > 1)
{
points1 = points1.reshape(1, npoints);
points2 = points2.reshape(1, npoints);
}
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double cx = cameraMatrix.at<double>(0,2);
double cy = cameraMatrix.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx) / fx;
points2.col(0) = (points2.col(0) - cx) / fx;
points1.col(1) = (points1.col(1) - cy) / fy;
points2.col(1) = (points2.col(1) - cy) / fy;
points1 = points1.t();
points2 = points2.t();
Mat R1, R2, t;
decomposeEssentialMat(E, R1, R2, t);
Mat P0 = Mat::eye(3, 4, R1.type());
Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());
P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;
P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;
P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
// Do the cheirality check.
// Notice here a threshold dist is used to filter
// out far away points (i.e. infinite points) since
// their depth may vary between positive and negative.
std::vector<Mat> allTriangulations(4);
Mat Q;
triangulatePoints(P0, P1, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[0]);
Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask1 = (Q.row(2) < distanceThresh) & mask1;
Q = P1 * Q;
mask1 = (Q.row(2) > 0) & mask1;
mask1 = (Q.row(2) < distanceThresh) & mask1;
triangulatePoints(P0, P2, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[1]);
Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask2 = (Q.row(2) < distanceThresh) & mask2;
Q = P2 * Q;
mask2 = (Q.row(2) > 0) & mask2;
mask2 = (Q.row(2) < distanceThresh) & mask2;
triangulatePoints(P0, P3, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[2]);
Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask3 = (Q.row(2) < distanceThresh) & mask3;
Q = P3 * Q;
mask3 = (Q.row(2) > 0) & mask3;
mask3 = (Q.row(2) < distanceThresh) & mask3;
triangulatePoints(P0, P4, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[3]);
Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask4 = (Q.row(2) < distanceThresh) & mask4;
Q = P4 * Q;
mask4 = (Q.row(2) > 0) & mask4;
mask4 = (Q.row(2) < distanceThresh) & mask4;
mask1 = mask1.t();
mask2 = mask2.t();
mask3 = mask3.t();
mask4 = mask4.t();
// If _mask is given, then use it to filter outliers.
if (!_mask.empty())
{
Mat mask = _mask.getMat();
CV_Assert(npoints == mask.checkVector(1));
mask = mask.reshape(1, npoints);
bitwise_and(mask, mask1, mask1);
bitwise_and(mask, mask2, mask2);
bitwise_and(mask, mask3, mask3);
bitwise_and(mask, mask4, mask4);
}
if (_mask.empty() && _mask.needed())
{
_mask.create(mask1.size(), CV_8U);
}
CV_Assert(_R.needed() && _t.needed());
_R.create(3, 3, R1.type());
_t.create(3, 1, t.type());
int good1 = countNonZero(mask1);
int good2 = countNonZero(mask2);
int good3 = countNonZero(mask3);
int good4 = countNonZero(mask4);
if (good1 >= good2 && good1 >= good3 && good1 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[0].copyTo(triangulatedPoints);
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask1.copyTo(_mask);
return good1;
}
else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[1].copyTo(triangulatedPoints);
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask2.copyTo(_mask);
return good2;
}
else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[2].copyTo(triangulatedPoints);
t = -t;
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask3.copyTo(_mask);
return good3;
}
else
{
if(triangulatedPoints.needed()) allTriangulations[3].copyTo(triangulatedPoints);
t = -t;
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask4.copyTo(_mask);
return good4;
}
}
int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
OutputArray _R, OutputArray _t, InputOutputArray _mask)
{
return cv::recoverPose(E, _points1, _points2, _cameraMatrix, _R, _t, 50, _mask);
}
int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2, OutputArray _R,
OutputArray _t, double focal, Point2d pp, InputOutputArray _mask)
{
Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);
return cv::recoverPose(E, _points1, _points2, cameraMatrix, _R, _t, _mask);
}
void cv::decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
{
CV_INSTRUMENT_REGION();
Mat E = _E.getMat().reshape(1, 3);
CV_Assert(E.cols == 3 && E.rows == 3);
Mat D, U, Vt;
SVD::compute(E, D, U, Vt);
if (determinant(U) < 0) U *= -1.;
if (determinant(Vt) < 0) Vt *= -1.;
Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
W.convertTo(W, E.type());
Mat R1, R2, t;
R1 = U * W * Vt;
R2 = U * W.t() * Vt;
t = U.col(2) * 1.0;
R1.copyTo(_R1);
R2.copyTo(_R2);
t.copyTo(_t);
}

1140
Lib/opencv/sources/modules/calib3d/src/fundam.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

565
Lib/opencv/sources/modules/calib3d/src/homography_decomp.cpp Normal file
View File

@@ -0,0 +1,565 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// This is a homography decomposition implementation contributed to OpenCV
// by Samson Yilma. It implements the homography decomposition algorithm
// described in the research report:
// Malis, E and Vargas, M, "Deeper understanding of the homography decomposition
// for vision-based control", Research Report 6303, INRIA (2007)
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, Samson Yilma (samson_yilma@yahoo.com), all rights reserved.
// Copyright (C) 2018, Intel Corporation, all rights reserved.
//
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <memory>
namespace cv
{
namespace HomographyDecomposition
{
//struct to hold solutions of homography decomposition
typedef struct _CameraMotion {
cv::Matx33d R; //!< rotation matrix
cv::Vec3d n; //!< normal of the plane the camera is looking at
cv::Vec3d t; //!< translation vector
} CameraMotion;
inline int signd(const double x)
{
return ( x >= 0 ? 1 : -1 );
}
class HomographyDecomp {
public:
HomographyDecomp() {}
virtual ~HomographyDecomp() {}
virtual void decomposeHomography(const cv::Matx33d& H, const cv::Matx33d& K,
std::vector<CameraMotion>& camMotions);
bool isRotationValid(const cv::Matx33d& R, const double epsilon=0.01);
protected:
bool passesSameSideOfPlaneConstraint(CameraMotion& motion);
virtual void decompose(std::vector<CameraMotion>& camMotions) = 0;
const cv::Matx33d& getHnorm() const {
return _Hnorm;
}
private:
/**
* Normalize the homograhpy \f$H\f$.
*
* @param H Homography matrix.
* @param K Intrinsic parameter matrix.
* @return It returns
* \f[
* K^{-1} * H * K
* \f]
*/
cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K);
void removeScale();
cv::Matx33d _Hnorm;
};
class HomographyDecompZhang CV_FINAL : public HomographyDecomp {
public:
HomographyDecompZhang():HomographyDecomp() {}
virtual ~HomographyDecompZhang() {}
private:
virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion);
};
class HomographyDecompInria CV_FINAL : public HomographyDecomp {
public:
HomographyDecompInria():HomographyDecomp() {}
virtual ~HomographyDecompInria() {}
private:
virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE;
double oppositeOfMinor(const cv::Matx33d& M, const int row, const int col);
void findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R);
};
// normalizes homography with intrinsic camera parameters
Matx33d HomographyDecomp::normalize(const Matx33d& H, const Matx33d& K)
{
return K.inv() * H * K;
}
void HomographyDecomp::removeScale()
{
Mat W;
SVD::compute(_Hnorm, W);
_Hnorm = _Hnorm * (1.0/W.at<double>(1));
}
/*! This checks that the input is a pure rotation matrix 'm'.
* The conditions for this are: R' * R = I and det(R) = 1 (proper rotation matrix)
*/
bool HomographyDecomp::isRotationValid(const Matx33d& R, const double epsilon)
{
Matx33d RtR = R.t() * R;
Matx33d I(1,0,0, 0,1,0, 0,0,1);
if (norm(RtR, I, NORM_INF) > epsilon)
return false;
return (fabs(determinant(R) - 1.0) < epsilon);
}
bool HomographyDecomp::passesSameSideOfPlaneConstraint(CameraMotion& motion)
{
typedef Matx<double, 1, 1> Matx11d;
Matx31d t = Matx31d(motion.t);
Matx31d n = Matx31d(motion.n);
Matx11d proj = n.t() * motion.R.t() * t;
if ( (1 + proj(0, 0) ) <= 0 )
return false;
return true;
}
//!main routine to decompose homography
void HomographyDecomp::decomposeHomography(const Matx33d& H, const cv::Matx33d& K,
std::vector<CameraMotion>& camMotions)
{
//normalize homography matrix with intrinsic camera matrix
_Hnorm = normalize(H, K);
//remove scale of the normalized homography
removeScale();
//apply decomposition
decompose(camMotions);
}
/* function computes R&t from tstar, and plane normal(n) using
R = H * inv(I + tstar*transpose(n) );
t = R * tstar;
returns true if computed R&t is a valid solution
*/
bool HomographyDecompZhang::findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion)
{
Matx31d tstar_m = Mat(tstar);
Matx31d n_m = Mat(n);
Matx33d temp = tstar_m * n_m.t();
temp(0, 0) += 1.0;
temp(1, 1) += 1.0;
temp(2, 2) += 1.0;
motion.R = getHnorm() * temp.inv();
if (cv::determinant(motion.R) < 0)
{
motion.R *= -1;
}
motion.t = motion.R * tstar;
motion.n = n;
return passesSameSideOfPlaneConstraint(motion);
}
void HomographyDecompZhang::decompose(std::vector<CameraMotion>& camMotions)
{
Mat W, U, Vt;
SVD::compute(getHnorm(), W, U, Vt);
CV_Assert(W.total() > 2 && Vt.total() > 7);
double lambda1=W.at<double>(0);
double lambda3=W.at<double>(2);
double lambda1m3 = (lambda1-lambda3);
double lambda1m3_2 = lambda1m3*lambda1m3;
double lambda1t3 = lambda1*lambda3;
double t1 = 1.0/(2.0*lambda1t3);
double t2 = sqrt(1.0+4.0*lambda1t3/lambda1m3_2);
double t12 = t1*t2;
double e1 = -t1 + t12; //t1*(-1.0f + t2 );
double e3 = -t1 - t12; //t1*(-1.0f - t2);
double e1_2 = e1*e1;
double e3_2 = e3*e3;
double nv1p = sqrt(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0);
double nv3p = sqrt(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0);
double v1p[3], v3p[3];
v1p[0]=Vt.at<double>(0)*nv1p, v1p[1]=Vt.at<double>(1)*nv1p, v1p[2]=Vt.at<double>(2)*nv1p;
v3p[0]=Vt.at<double>(6)*nv3p, v3p[1]=Vt.at<double>(7)*nv3p, v3p[2]=Vt.at<double>(8)*nv3p;
/*The eight solutions are
(A): tstar = +- (v1p - v3p)/(e1 -e3), n = +- (e1*v3p - e3*v1p)/(e1-e3)
(B): tstar = +- (v1p + v3p)/(e1 -e3), n = +- (e1*v3p + e3*v1p)/(e1-e3)
*/
double v1pmv3p[3], v1ppv3p[3];
double e1v3me3v1[3], e1v3pe3v1[3];
double inv_e1me3 = 1.0/(e1-e3);
for(int kk=0;kk<3;++kk){
v1pmv3p[kk] = v1p[kk]-v3p[kk];
v1ppv3p[kk] = v1p[kk]+v3p[kk];
}
for(int kk=0; kk<3; ++kk){
double e1v3 = e1*v3p[kk];
double e3v1=e3*v1p[kk];
e1v3me3v1[kk] = e1v3-e3v1;
e1v3pe3v1[kk] = e1v3+e3v1;
}
Vec3d tstar_p, tstar_n;
Vec3d n_p, n_n;
///Solution group A
for(int kk=0; kk<3; ++kk) {
tstar_p[kk] = v1pmv3p[kk]*inv_e1me3;
tstar_n[kk] = -tstar_p[kk];
n_p[kk] = e1v3me3v1[kk]*inv_e1me3;
n_n[kk] = -n_p[kk];
}
CameraMotion cmotion;
//(A) Four different combinations for solution A
// (i) (+, +)
if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
camMotions.push_back(cmotion);
// (ii) (+, -)
if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
camMotions.push_back(cmotion);
// (iii) (-, +)
if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
camMotions.push_back(cmotion);
// (iv) (-, -)
if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
camMotions.push_back(cmotion);
//////////////////////////////////////////////////////////////////
///Solution group B
for(int kk=0;kk<3;++kk){
tstar_p[kk] = v1ppv3p[kk]*inv_e1me3;
tstar_n[kk] = -tstar_p[kk];
n_p[kk] = e1v3pe3v1[kk]*inv_e1me3;
n_n[kk] = -n_p[kk];
}
//(B) Four different combinations for solution B
// (i) (+, +)
if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
camMotions.push_back(cmotion);
// (ii) (+, -)
if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
camMotions.push_back(cmotion);
// (iii) (-, +)
if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
camMotions.push_back(cmotion);
// (iv) (-, -)
if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
camMotions.push_back(cmotion);
}
double HomographyDecompInria::oppositeOfMinor(const Matx33d& M, const int row, const int col)
{
int x1 = col == 0 ? 1 : 0;
int x2 = col == 2 ? 1 : 2;
int y1 = row == 0 ? 1 : 0;
int y2 = row == 2 ? 1 : 2;
return (M(y1, x2) * M(y2, x1) - M(y1, x1) * M(y2, x2));
}
//computes R = H( I - (2/v)*te_star*ne_t )
void HomographyDecompInria::findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R)
{
Matx31d tstar_m = Matx31d(tstar);
Matx31d n_m = Matx31d(n);
Matx33d I(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
R = getHnorm() * (I - (2/v) * tstar_m * n_m.t() );
if (cv::determinant(R) < 0)
{
R *= -1;
}
}
void HomographyDecompInria::decompose(std::vector<CameraMotion>& camMotions)
{
const double epsilon = 0.001;
Matx33d S;
//S = H'H - I
S = getHnorm().t() * getHnorm();
S(0, 0) -= 1.0;
S(1, 1) -= 1.0;
S(2, 2) -= 1.0;
//check if H is rotation matrix
if( norm(S, NORM_INF) < epsilon) {
CameraMotion motion;
motion.R = Matx33d(getHnorm());
motion.t = Vec3d(0, 0, 0);
motion.n = Vec3d(0, 0, 0);
camMotions.push_back(motion);
return;
}
//! Compute nvectors
Vec3d npa, npb;
double M00 = oppositeOfMinor(S, 0, 0);
double M11 = oppositeOfMinor(S, 1, 1);
double M22 = oppositeOfMinor(S, 2, 2);
double rtM00 = sqrt(M00);
double rtM11 = sqrt(M11);
double rtM22 = sqrt(M22);
double M01 = oppositeOfMinor(S, 0, 1);
double M12 = oppositeOfMinor(S, 1, 2);
double M02 = oppositeOfMinor(S, 0, 2);
int e12 = signd(M12);
int e02 = signd(M02);
int e01 = signd(M01);
double nS00 = abs(S(0, 0));
double nS11 = abs(S(1, 1));
double nS22 = abs(S(2, 2));
//find max( |Sii| ), i=0, 1, 2
int indx = 0;
if(nS00 < nS11){
indx = 1;
if( nS11 < nS22 )
indx = 2;
}
else {
if(nS00 < nS22 )
indx = 2;
}
switch (indx) {
case 0:
npa[0] = S(0, 0), npb[0] = S(0, 0);
npa[1] = S(0, 1) + rtM22, npb[1] = S(0, 1) - rtM22;
npa[2] = S(0, 2) + e12 * rtM11, npb[2] = S(0, 2) - e12 * rtM11;
break;
case 1:
npa[0] = S(0, 1) + rtM22, npb[0] = S(0, 1) - rtM22;
npa[1] = S(1, 1), npb[1] = S(1, 1);
npa[2] = S(1, 2) - e02 * rtM00, npb[2] = S(1, 2) + e02 * rtM00;
break;
case 2:
npa[0] = S(0, 2) + e01 * rtM11, npb[0] = S(0, 2) - e01 * rtM11;
npa[1] = S(1, 2) + rtM00, npb[1] = S(1, 2) - rtM00;
npa[2] = S(2, 2), npb[2] = S(2, 2);
break;
default:
break;
}
double traceS = S(0, 0) + S(1, 1) + S(2, 2);
double v = 2.0 * sqrt(1 + traceS - M00 - M11 - M22);
double ESii = signd(S(indx, indx)) ;
double r_2 = 2 + traceS + v;
double nt_2 = 2 + traceS - v;
double r = sqrt(r_2);
double n_t = sqrt(nt_2);
Vec3d na = npa / norm(npa);
Vec3d nb = npb / norm(npb);
double half_nt = 0.5 * n_t;
double esii_t_r = ESii * r;
Vec3d ta_star = half_nt * (esii_t_r * nb - n_t * na);
Vec3d tb_star = half_nt * (esii_t_r * na - n_t * nb);
camMotions.resize(4);
Matx33d Ra, Rb;
Vec3d ta, tb;
//Ra, ta, na
findRmatFrom_tstar_n(ta_star, na, v, Ra);
ta = Ra * ta_star;
camMotions[0].R = Ra;
camMotions[0].t = ta;
camMotions[0].n = na;
//Ra, -ta, -na
camMotions[1].R = Ra;
camMotions[1].t = -ta;
camMotions[1].n = -na;
//Rb, tb, nb
findRmatFrom_tstar_n(tb_star, nb, v, Rb);
tb = Rb * tb_star;
camMotions[2].R = Rb;
camMotions[2].t = tb;
camMotions[2].n = nb;
//Rb, -tb, -nb
camMotions[3].R = Rb;
camMotions[3].t = -tb;
camMotions[3].n = -nb;
}
} //namespace HomographyDecomposition
// function decomposes image-to-image homography to rotation and translation matrices
int decomposeHomographyMat(InputArray _H,
InputArray _K,
OutputArrayOfArrays _rotations,
OutputArrayOfArrays _translations,
OutputArrayOfArrays _normals)
{
using namespace std;
using namespace HomographyDecomposition;
Mat H = _H.getMat().reshape(1, 3);
CV_Assert(H.cols == 3 && H.rows == 3);
Mat K = _K.getMat().reshape(1, 3);
CV_Assert(K.cols == 3 && K.rows == 3);
cv::Ptr<HomographyDecomp> hdecomp(new HomographyDecompInria);
vector<CameraMotion> motions;
hdecomp->decomposeHomography(H, K, motions);
int nsols = static_cast<int>(motions.size());
int depth = CV_64F; //double precision matrices used in CameraMotion struct
if (_rotations.needed()) {
_rotations.create(nsols, 1, depth);
for (int k = 0; k < nsols; ++k ) {
_rotations.getMatRef(k) = Mat(motions[k].R);
}
}
if (_translations.needed()) {
_translations.create(nsols, 1, depth);
for (int k = 0; k < nsols; ++k ) {
_translations.getMatRef(k) = Mat(motions[k].t);
}
}
if (_normals.needed()) {
_normals.create(nsols, 1, depth);
for (int k = 0; k < nsols; ++k ) {
_normals.getMatRef(k) = Mat(motions[k].n);
}
}
return nsols;
}
void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays _rotations,
InputArrayOfArrays _normals,
InputArray _beforeRectifiedPoints,
InputArray _afterRectifiedPoints,
OutputArray _possibleSolutions,
InputArray _pointsMask)
{
CV_Assert(_beforeRectifiedPoints.type() == CV_32FC2 && _afterRectifiedPoints.type() == CV_32FC2);
CV_Assert(_pointsMask.empty() || _pointsMask.type() == CV_8U);
Mat beforeRectifiedPoints = _beforeRectifiedPoints.getMat();
Mat afterRectifiedPoints = _afterRectifiedPoints.getMat();
Mat pointsMask = _pointsMask.getMat();
int nsolutions = (int)_rotations.total();
int npoints = (int)beforeRectifiedPoints.total();
CV_Assert(pointsMask.empty() || pointsMask.checkVector(1, CV_8U) == npoints);
const uchar* pointsMaskPtr = pointsMask.data;
std::vector<uchar> solutionMask(nsolutions, (uchar)1);
std::vector<Mat> normals(nsolutions);
std::vector<Mat> rotnorm(nsolutions);
Mat R;
for( int i = 0; i < nsolutions; i++ )
{
_normals.getMat(i).convertTo(normals[i], CV_64F);
CV_Assert(normals[i].total() == 3);
_rotations.getMat(i).convertTo(R, CV_64F);
rotnorm[i] = R*normals[i];
CV_Assert(rotnorm[i].total() == 3);
}
for( int j = 0; j < npoints; j++ )
{
if( !pointsMaskPtr || pointsMaskPtr[j] )
{
Point2f prevPoint = beforeRectifiedPoints.at<Point2f>(j);
Point2f currPoint = afterRectifiedPoints.at<Point2f>(j);
for( int i = 0; i < nsolutions; i++ )
{
if( !solutionMask[i] )
continue;
const double* normal_i = normals[i].ptr<double>();
const double* rotnorm_i = rotnorm[i].ptr<double>();
double prevNormDot = normal_i[0]*prevPoint.x + normal_i[1]*prevPoint.y + normal_i[2];
double currNormDot = rotnorm_i[0]*currPoint.x + rotnorm_i[1]*currPoint.y + rotnorm_i[2];
if (prevNormDot <= 0 || currNormDot <= 0)
solutionMask[i] = (uchar)0;
}
}
}
std::vector<int> possibleSolutions;
for( int i = 0; i < nsolutions; i++ )
if( solutionMask[i] )
possibleSolutions.push_back(i);
Mat(possibleSolutions).copyTo(_possibleSolutions);
}
} //namespace cv

1100
Lib/opencv/sources/modules/calib3d/src/ippe.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

259
Lib/opencv/sources/modules/calib3d/src/ippe.hpp Normal file
View File

@@ -0,0 +1,259 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This file is based on file issued with the following license:
/*============================================================================
Copyright 2017 Toby Collins
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef OPENCV_CALIB3D_IPPE_HPP
#define OPENCV_CALIB3D_IPPE_HPP
#include <opencv2/core.hpp>
namespace cv {
namespace IPPE {
class PoseSolver {
public:
/**
* @brief PoseSolver constructor
*/
PoseSolver();
/**
* @brief Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors.
* The poses are sorted with the first having the lowest reprojection error.
* @param objectPoints Array of 4 or more coplanar object points defined in object coordinates.
* 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates.
* @param rvec1 First rotation solution (3x1 rotation vector)
* @param tvec1 First translation solution (3x1 vector)
* @param reprojErr1 Reprojection error of first solution
* @param rvec2 Second rotation solution (3x1 rotation vector)
* @param tvec2 Second translation solution (3x1 vector)
* @param reprojErr2 Reprojection error of second solution
*/
void solveGeneric(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1,
float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2);
/**
* @brief Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE.
* The poses are sorted so that the first one is the one with the lowest reprojection error.
*
* @param objectPoints Array of 4 coplanar object points defined in the following object coordinates:
* - point 0: [-squareLength / 2.0, squareLength / 2.0, 0]
* - point 1: [squareLength / 2.0, squareLength / 2.0, 0]
* - point 2: [squareLength / 2.0, -squareLength / 2.0, 0]
* - point 3: [-squareLength / 2.0, -squareLength / 2.0, 0]
* 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates.
* @param rvec1 First rotation solution (3x1 rotation vector)
* @param tvec1 First translation solution (3x1 vector)
* @param reprojErr1 Reprojection error of first solution
* @param rvec2 Second rotation solution (3x1 rotation vector)
* @param tvec2 Second translation solution (3x1 vector)
* @param reprojErr2 Reprojection error of second solution
*/
void solveSquare(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1,
float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2);
private:
/**
* @brief Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates.
* These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
* @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double).
* @param normalizedImagePoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double).
* @param Ma First pose solution (unsorted)
* @param Mb Second pose solution (unsorted)
*/
void solveGeneric(InputArray objectPoints, InputArray normalizedImagePoints, OutputArray Ma, OutputArray Mb);
/**
* @brief Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates.
* These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
* @param canonicalObjPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points
* @param normalizedInputPoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points
* @param H Homography mapping canonicalObjPoints to normalizedInputPoints.
* @param Ma
* @param Mb
*/
void solveCanonicalForm(InputArray canonicalObjPoints, InputArray normalizedInputPoints, const Matx33d& H,
OutputArray Ma, OutputArray Mb);
/**
* @brief Computes the translation solution for a given rotation solution
* @param objectPoints Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points
* @param normalizedImagePoints Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points
* @param R Rotation solution (3x1 rotation vector)
* @param t Translation solution (3x1 rotation vector)
*/
void computeTranslation(InputArray objectPoints, InputArray normalizedImgPoints, InputArray R, OutputArray t);
/**
* @brief Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane.
* For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this).
* For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates).
* The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy).
* @param j00 Homography jacobian coefficient at (ux,uy)
* @param j01 Homography jacobian coefficient at (ux,uy)
* @param j10 Homography jacobian coefficient at (ux,uy)
* @param j11 Homography jacobian coefficient at (ux,uy)
* @param p The x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
* @param q The y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
*/
void computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2);
/**
* @brief Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points).
* The source points are the four corners of a zero-centred squared defined by:
* - point 0: [-squareLength / 2.0, squareLength / 2.0]
* - point 1: [squareLength / 2.0, squareLength / 2.0]
* - point 2: [squareLength / 2.0, -squareLength / 2.0]
* - point 3: [-squareLength / 2.0, -squareLength / 2.0]
*
* @param targetPoints Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3.
* @param halfLength The square's half length (i.e. squareLength/2.0)
* @param H Homograhy mapping the source points to the target points, 3x3 single channel
*/
void homographyFromSquarePoints(InputArray targetPoints, double halfLength, OutputArray H);
/**
* @brief Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula
* @param R Input rotation matrix, 3x3 1-channel (double)
* @param r Output rotation vector, 3x1/1x3 1-channel (double)
*/
void rot2vec(InputArray R, OutputArray r);
/**
* @brief Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0
* @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param canonicalObjectPoints Object points in canonical coordinates 1xN/Nx1 2-channel (double)
* @param MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double)
*/
void makeCanonicalObjectPoints(InputArray objectPoints, OutputArray canonicalObjectPoints, OutputArray MobjectPoints2Canonical);
/**
* @brief Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution.
* @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
* @param M Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double)
* @param err RMS reprojection error
*/
void evalReprojError(InputArray objectPoints, InputArray imagePoints, InputArray M, float& err);
/**
* @brief Sorts two pose solutions according to their RMS reprojection error (lowest first).
* @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
* @param Ma Pose matrix 1: 4x4 1-channel
* @param Mb Pose matrix 2: 4x4 1-channel
* @param M1 Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy.
* @param M2 Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy.
* @param err1 RMS reprojection error of _M1
* @param err2 RMS reprojection error of _M2
*/
void sortPosesByReprojError(InputArray objectPoints, InputArray imagePoints, InputArray Ma, InputArray Mb, OutputArray M1, OutputArray M2, float& err1, float& err2);
/**
* @brief Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1)
* @param a vector: 3x1 mat (double)
* @param Ra Rotation: 3x3 mat (double)
*/
void rotateVec2ZAxis(const Matx31d& a, Matx33d& Ra);
/**
* @brief Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
* @param objectPoints Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
* @param R Rotation Mat: 3x3 (double)
* @return Success (true) or failure (false)
*/
bool computeObjextSpaceR3Pts(InputArray objectPoints, Matx33d& R);
/**
* @brief computeObjextSpaceRSvD Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
* @param objectPointsZeroMean Zero-meaned coplanar object points: 3xN matrix (double) where N>=3
* @param R Rotation Mat: 3x3 (double)
*/
void computeObjextSpaceRSvD(InputArray objectPointsZeroMean, OutputArray R);
/**
* @brief Generates the 4 object points of a square planar object
* @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
* @param objectPoints Set of 4 object points (1x4 3-channel double)
*/
void generateSquareObjectCorners3D(double squareLength, OutputArray objectPoints);
/**
* @brief Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points).
* @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
* @param objectPoints Set of 4 object points (1x4 2-channel double)
*/
void generateSquareObjectCorners2D(double squareLength, OutputArray objectPoints);
/**
* @brief Computes the average depth of an object given its pose in camera coordinates
* @param objectPoints: Object points defined in 3D object space
* @param rvec: Rotation component of pose
* @param tvec: Translation component of pose
* @return: average depth of the object
*/
double meanSceneDepth(InputArray objectPoints, InputArray rvec, InputArray tvec);
//! a small constant used to test 'small' values close to zero.
double IPPE_SMALL;
};
} //namespace IPPE
namespace HomographyHO {
/**
* @brief Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method:
* Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England.
* This is not the author's implementation.
* @param srcPoints Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points
* @param targPoints Array of target points: 1xN/Nx1 2-channel (float or double)
* @param H Homography from source to target: 3x3 1-channel (double)
*/
void homographyHO(InputArray srcPoints, InputArray targPoints, Matx33d& H);
/**
* @brief Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision,
* Cambridge University Press, Cambridge, 2001
* @param Data Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
* @param DataN Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
* @param T Homogeneous transform from source to normalized: 3x3 1-channel (double)
* @param Ti Homogeneous transform from normalized to source: 3x3 1-channel (double)
*/
void normalizeDataIsotropic(InputArray Data, OutputArray DataN, OutputArray T, OutputArray Ti);
}
} //namespace cv
#endif

220
Lib/opencv/sources/modules/calib3d/src/levmarq.cpp Normal file
View File

@@ -0,0 +1,220 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <stdio.h>
/*
This is a translation to C++ from the Matlab's LMSolve package by Miroslav Balda.
Here is the original copyright:
============================================================================
Copyright (c) 2007, Miroslav Balda
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the distribution
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
namespace cv
{
class LMSolverImpl CV_FINAL : public LMSolver
{
public:
LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps = FLT_EPSILON)
: cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters)
{
printInterval = 0;
}
int run(InputOutputArray _param0) const CV_OVERRIDE
{
Mat param0 = _param0.getMat(), x, xd, r, rd, J, A, Ap, v, temp_d, d;
int ptype = param0.type();
CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F));
CV_Assert( cb );
int lx = param0.rows + param0.cols - 1;
param0.convertTo(x, CV_64F);
if( x.cols != 1 )
transpose(x, x);
if( !cb->compute(x, r, J) )
return -1;
double S = norm(r, NORM_L2SQR);
int nfJ = 2;
mulTransposed(J, A, true);
gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
Mat D = A.diag().clone();
const double Rlo = 0.25, Rhi = 0.75;
double lambda = 1, lc = 0.75;
int i, iter = 0;
if( printInterval != 0 )
{
printf("************************************************************************************\n");
printf("\titr\tnfJ\t\tSUM(r^2)\t\tx\t\tdx\t\tl\t\tlc\n");
printf("************************************************************************************\n");
}
for( ;; )
{
CV_Assert( A.type() == CV_64F && A.rows == lx );
A.copyTo(Ap);
for( i = 0; i < lx; i++ )
Ap.at<double>(i, i) += lambda*D.at<double>(i);
solve(Ap, v, d, DECOMP_EIG);
subtract(x, d, xd);
if( !cb->compute(xd, rd, noArray()) )
return -1;
nfJ++;
double Sd = norm(rd, NORM_L2SQR);
gemm(A, d, -1, v, 2, temp_d);
double dS = d.dot(temp_d);
double R = (S - Sd)/(fabs(dS) > DBL_EPSILON ? dS : 1);
if( R > Rhi )
{
lambda *= 0.5;
if( lambda < lc )
lambda = 0;
}
else if( R < Rlo )
{
// find new nu if R too low
double t = d.dot(v);
double nu = (Sd - S)/(fabs(t) > DBL_EPSILON ? t : 1) + 2;
nu = std::min(std::max(nu, 2.), 10.);
if( lambda == 0 )
{
invert(A, Ap, DECOMP_EIG);
double maxval = DBL_EPSILON;
for( i = 0; i < lx; i++ )
maxval = std::max(maxval, std::abs(Ap.at<double>(i,i)));
lambda = lc = 1./maxval;
nu *= 0.5;
}
lambda *= nu;
}
if( Sd < S )
{
nfJ++;
S = Sd;
std::swap(x, xd);
if( !cb->compute(x, r, J) )
return -1;
mulTransposed(J, A, true);
gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
}
iter++;
bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && norm(r, NORM_INF) >= epsf;
if( printInterval != 0 && (iter % printInterval == 0 || iter == 1 || !proceed) )
{
printf("%c%10d %10d %15.4e %16.4e %17.4e %16.4e %17.4e\n",
(proceed ? ' ' : '*'), iter, nfJ, S, x.at<double>(0), d.at<double>(0), lambda, lc);
}
if(!proceed)
break;
}
if( param0.size != x.size )
transpose(x, x);
x.convertTo(param0, ptype);
if( iter == maxIters )
iter = -iter;
return iter;
}
void setMaxIters(int iters) CV_OVERRIDE { CV_Assert(iters > 0); maxIters = iters; }
int getMaxIters() const CV_OVERRIDE { return maxIters; }
Ptr<LMSolver::Callback> cb;
double epsx;
double epsf;
int maxIters;
int printInterval;
};
Ptr<LMSolver> LMSolver::create(const Ptr<LMSolver::Callback>& cb, int maxIters)
{
return makePtr<LMSolverImpl>(cb, maxIters);
}
Ptr<LMSolver> LMSolver::create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
{
return makePtr<LMSolverImpl>(cb, maxIters, eps);
}
}

52
Lib/opencv/sources/modules/calib3d/src/main.cpp Normal file
View File

@@ -0,0 +1,52 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Copyright (C) 2015, Itseez Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
//
// Library initialization file
//
#include "precomp.hpp"
IPP_INITIALIZER_AUTO
/* End of file. */

334
Lib/opencv/sources/modules/calib3d/src/opencl/stereobm.cl Normal file
View File

@@ -0,0 +1,334 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2010-2012, Institute Of Software Chinese Academy Of Science, all rights reserved.
// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors as is and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
//////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////// stereoBM //////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
#define MAX_VAL 32767
#ifndef WSZ
#define WSZ 2
#endif
#define WSZ2 (WSZ / 2)
#ifdef DEFINE_KERNEL_STEREOBM
#define DISPARITY_SHIFT 4
#define FILTERED ((MIN_DISP - 1) << DISPARITY_SHIFT)
void calcDisp(__local short * cost, __global short * disp, int uniquenessRatio,
__local int * bestDisp, __local int * bestCost, int d, int x, int y, int cols, int rows)
{
int best_disp = *bestDisp, best_cost = *bestCost;
barrier(CLK_LOCAL_MEM_FENCE);
short c = cost[0];
int thresh = best_cost + (best_cost * uniquenessRatio / 100);
bool notUniq = ( (c <= thresh) && (d < (best_disp - 1) || d > (best_disp + 1) ) );
if (notUniq)
*bestCost = FILTERED;
barrier(CLK_LOCAL_MEM_FENCE);
if( *bestCost != FILTERED && x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2 && d == best_disp)
{
int d_aprox = 0;
int yp =0, yn = 0;
if ((0 < best_disp) && (best_disp < NUM_DISP - 1))
{
yp = cost[-2 * BLOCK_SIZE_Y];
yn = cost[2 * BLOCK_SIZE_Y];
d_aprox = yp + yn - 2 * c + abs(yp - yn);
}
disp[0] = (short)(((best_disp + MIN_DISP)*256 + (d_aprox != 0 ? (yp - yn) * 256 / d_aprox : 0) + 15) >> 4);
}
}
short calcCostBorder(__global const uchar * leftptr, __global const uchar * rightptr, int x, int y, int nthread,
short * costbuf, int *h, int cols, int d, short cost)
{
int head = (*h) % WSZ;
__global const uchar * left, * right;
int idx = mad24(y + WSZ2 * (2 * nthread - 1), cols, x + WSZ2 * (1 - 2 * nthread));
left = leftptr + idx;
right = rightptr + (idx - d);
short costdiff = 0;
if (0 == nthread)
{
#pragma unroll
for (int i = 0; i < WSZ; i++)
{
costdiff += abs( left[0] - right[0] );
left += cols;
right += cols;
}
}
else // (1 == nthread)
{
#pragma unroll
for (int i = 0; i < WSZ; i++)
{
costdiff += abs(left[i] - right[i]);
}
}
cost += costdiff - costbuf[head];
costbuf[head] = costdiff;
*h = head + 1;
return cost;
}
short calcCostInside(__global const uchar * leftptr, __global const uchar * rightptr, int x, int y,
int cols, int d, short cost_up_left, short cost_up, short cost_left)
{
__global const uchar * left, * right;
int idx = mad24(y - WSZ2 - 1, cols, x - WSZ2 - 1);
left = leftptr + idx;
right = rightptr + (idx - d);
int idx2 = WSZ*cols;
uchar corrner1 = abs(left[0] - right[0]),
corrner2 = abs(left[WSZ] - right[WSZ]),
corrner3 = abs(left[idx2] - right[idx2]),
corrner4 = abs(left[idx2 + WSZ] - right[idx2 + WSZ]);
return cost_up + cost_left - cost_up_left + corrner1 -
corrner2 - corrner3 + corrner4;
}
__kernel void stereoBM(__global const uchar * leftptr,
__global const uchar * rightptr,
__global uchar * dispptr, int disp_step, int disp_offset,
int rows, int cols, // rows, cols of left and right images, not disp
int textureThreshold, int uniquenessRatio)
{
int lz = get_local_id(0);
int gx = get_global_id(1) * BLOCK_SIZE_X;
int gy = get_global_id(2) * BLOCK_SIZE_Y;
int nthread = lz / NUM_DISP;
int disp_idx = lz % NUM_DISP;
__global short * disp;
__global const uchar * left, * right;
__local short costFunc[2 * BLOCK_SIZE_Y * NUM_DISP];
__local short * cost;
__local int best_disp[2];
__local int best_cost[2];
best_cost[nthread] = MAX_VAL;
best_disp[nthread] = -1;
barrier(CLK_LOCAL_MEM_FENCE);
short costbuf[WSZ];
int head = 0;
int shiftX = WSZ2 + NUM_DISP + MIN_DISP - 1;
int shiftY = WSZ2;
int x = gx + shiftX, y = gy + shiftY, lx = 0, ly = 0;
int costIdx = disp_idx * 2 * BLOCK_SIZE_Y + (BLOCK_SIZE_Y - 1);
cost = costFunc + costIdx;
int tempcost = 0;
if (x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2)
{
if (0 == nthread)
{
#pragma unroll
for (int i = 0; i < WSZ; i++)
{
int idx = mad24(y - WSZ2, cols, x - WSZ2 + i);
left = leftptr + idx;
right = rightptr + (idx - disp_idx);
short costdiff = 0;
for(int j = 0; j < WSZ; j++)
{
costdiff += abs( left[0] - right[0] );
left += cols;
right += cols;
}
costbuf[i] = costdiff;
}
}
else // (1 == nthread)
{
#pragma unroll
for (int i = 0; i < WSZ; i++)
{
int idx = mad24(y - WSZ2 + i, cols, x - WSZ2);
left = leftptr + idx;
right = rightptr + (idx - disp_idx);
short costdiff = 0;
for (int j = 0; j < WSZ; j++)
{
costdiff += abs( left[j] - right[j]);
}
tempcost += costdiff;
costbuf[i] = costdiff;
}
}
}
if (nthread == 1)
{
cost[0] = tempcost;
atomic_min(best_cost + 1, tempcost);
}
barrier(CLK_LOCAL_MEM_FENCE);
if (best_cost[1] == tempcost)
atomic_max(best_disp + 1, disp_idx);
barrier(CLK_LOCAL_MEM_FENCE);
int dispIdx = mad24(gy, disp_step, mad24((int)sizeof(short), gx, disp_offset));
disp = (__global short *)(dispptr + dispIdx);
calcDisp(cost, disp, uniquenessRatio, best_disp + 1, best_cost + 1, disp_idx, x, y, cols, rows);
barrier(CLK_LOCAL_MEM_FENCE);
lx = 1 - nthread;
ly = nthread;
for (int i = 0; i < BLOCK_SIZE_Y * BLOCK_SIZE_X / 2; i++)
{
x = (lx < BLOCK_SIZE_X) ? gx + shiftX + lx : cols;
y = (ly < BLOCK_SIZE_Y) ? gy + shiftY + ly : rows;
best_cost[nthread] = MAX_VAL;
best_disp[nthread] = -1;
barrier(CLK_LOCAL_MEM_FENCE);
costIdx = mad24(2 * BLOCK_SIZE_Y, disp_idx, (BLOCK_SIZE_Y - 1 - ly + lx));
if (0 > costIdx)
costIdx = BLOCK_SIZE_Y - 1;
cost = costFunc + costIdx;
if (x < cols - WSZ2 - MIN_DISP && y < rows - WSZ2)
{
tempcost = (ly * (1 - nthread) + lx * nthread == 0) ?
calcCostBorder(leftptr, rightptr, x, y, nthread, costbuf, &head, cols, disp_idx, cost[2*nthread-1]) :
calcCostInside(leftptr, rightptr, x, y, cols, disp_idx, cost[0], cost[1], cost[-1]);
}
cost[0] = tempcost;
atomic_min(best_cost + nthread, tempcost);
barrier(CLK_LOCAL_MEM_FENCE);
if (best_cost[nthread] == tempcost)
atomic_max(best_disp + nthread, disp_idx);
barrier(CLK_LOCAL_MEM_FENCE);
dispIdx = mad24(gy + ly, disp_step, mad24((int)sizeof(short), (gx + lx), disp_offset));
disp = (__global short *)(dispptr + dispIdx);
calcDisp(cost, disp, uniquenessRatio, best_disp + nthread, best_cost + nthread, disp_idx, x, y, cols, rows);
barrier(CLK_LOCAL_MEM_FENCE);
if (lx + nthread - 1 == ly)
{
lx = (lx + nthread + 1) * (1 - nthread);
ly = (ly + 1) * nthread;
}
else
{
lx += nthread;
ly = ly - nthread + 1;
}
}
}
#endif //DEFINE_KERNEL_STEREOBM
//////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////// Norm Prefiler ////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
__kernel void prefilter_norm(__global unsigned char *input, __global unsigned char *output,
int rows, int cols, int prefilterCap, int scale_g, int scale_s)
{
// prefilterCap in range 1..63, checked in StereoBMImpl::compute
int x = get_global_id(0);
int y = get_global_id(1);
if(x < cols && y < rows)
{
int cov1 = input[ max(y-1, 0) * cols + x] * 1 +
input[y * cols + max(x-1,0)] * 1 + input[ y * cols + x] * 4 + input[y * cols + min(x+1, cols-1)] * 1 +
input[min(y+1, rows-1) * cols + x] * 1;
int cov2 = 0;
for(int i = -WSZ2; i < WSZ2+1; i++)
for(int j = -WSZ2; j < WSZ2+1; j++)
cov2 += input[clamp(y+i, 0, rows-1) * cols + clamp(x+j, 0, cols-1)];
int res = (cov1*scale_g - cov2*scale_s)>>10;
res = clamp(res, -prefilterCap, prefilterCap) + prefilterCap;
output[y * cols + x] = res;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////// Sobel Prefiler ////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
__kernel void prefilter_xsobel(__global unsigned char *input, __global unsigned char *output,
int rows, int cols, int prefilterCap)
{
// prefilterCap in range 1..63, checked in StereoBMImpl::compute
int x = get_global_id(0);
int y = get_global_id(1);
if(x < cols && y < rows)
{
if (0 < x && !((y == rows-1) & (rows%2==1) ) )
{
int cov = input[ ((y > 0) ? y-1 : y+1) * cols + (x-1)] * (-1) + input[ ((y > 0) ? y-1 : y+1) * cols + ((x<cols-1) ? x+1 : x-1)] * (1) +
input[ (y) * cols + (x-1)] * (-2) + input[ (y) * cols + ((x<cols-1) ? x+1 : x-1)] * (2) +
input[((y<rows-1)?(y+1):(y-1))* cols + (x-1)] * (-1) + input[((y<rows-1)?(y+1):(y-1))* cols + ((x<cols-1) ? x+1 : x-1)] * (1);
cov = clamp(cov, -prefilterCap, prefilterCap) + prefilterCap;
output[y * cols + x] = cov;
}
else
output[y * cols + x] = prefilterCap;
}
}

466
Lib/opencv/sources/modules/calib3d/src/p3p.cpp Normal file
View File

@@ -0,0 +1,466 @@
#include <cstring>
#include <cmath>
#include <iostream>
#include "polynom_solver.h"
#include "p3p.h"
void p3p::init_inverse_parameters()
{
inv_fx = 1. / fx;
inv_fy = 1. / fy;
cx_fx = cx / fx;
cy_fy = cy / fy;
}
p3p::p3p(cv::Mat cameraMatrix)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_inverse_parameters();
}
p3p::p3p(double _fx, double _fy, double _cx, double _cy)
{
fx = _fx;
fy = _fy;
cx = _cx;
cy = _cy;
init_inverse_parameters();
}
bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
{
CV_INSTRUMENT_REGION();
double rotation_matrix[3][3] = {}, translation[3] = {};
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
bool result = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
return result;
}
int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
{
CV_INSTRUMENT_REGION();
double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19],
p4p);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool p3p::solve(double R[3][3], double t[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)
{
double Rs[4][3][3] = {}, ts[4][3] = {};
const bool p4p = true;
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
if (n == 0)
return false;
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R[i][j] = Rs[0][i][j];
t[i] = ts[0][i];
}
return true;
}
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;
}
/// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
/// the lengths of the line segments connecting projection center (P) and the three 3D points (A, B, C).
/// Returned distances are for |PA|, |PB|, |PC| respectively.
/// Only the solution to the main branch.
/// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
/// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003
/// \param lengths3D Lengths of line segments up to four solutions.
/// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|.
/// \param cosines Cosine of the angles /_BPC, /_APC, /_APB.
/// \returns Number of solutions.
/// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED
int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
{
double p = cosines[0] * 2;
double q = cosines[1] * 2;
double r = cosines[2] * 2;
double inv_d22 = 1. / (distances[2] * distances[2]);
double a = inv_d22 * (distances[0] * distances[0]);
double b = inv_d22 * (distances[1] * distances[1]);
double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
double pr = p * r, pqr = q * pr;
// Check reality condition (the four points should not be coplanar)
if (p2 + q2 + r2 - pqr - 1 == 0)
return 0;
double ab = a * b, a_2 = 2*a;
double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2;
// Check reality condition
if (A == 0) return 0;
double a_4 = 4*a;
double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab);
double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2;
double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2);
double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2;
double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr);
double b0 = b * temp * temp;
// Check reality condition
if (b0 == 0)
return 0;
double real_roots[4];
int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]);
if (n == 0)
return 0;
int nb_solutions = 0;
double r3 = r2*r, pr2 = p*r2, r3q = r3 * q;
double inv_b0 = 1. / b0;
// For each solution of x
for(int i = 0; i < n; i++) {
double x = real_roots[i];
// Check reality condition
if (x <= 0)
continue;
double x2 = x*x;
double b1 =
((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) *
(((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x +
(r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 +
(r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x +
2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) +
p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1)));
// Check reality condition
if (b1 <= 0)
continue;
double y = inv_b0 * b1;
double v = x2 + y*y - x*y*r;
if (v <= 0)
continue;
double Z = distances[2] / sqrt(v);
double X = x * Z;
double Y = y * Z;
lengths[nb_solutions][0] = X;
lengths[nb_solutions][1] = Y;
lengths[nb_solutions][2] = Z;
nb_solutions++;
}
return nb_solutions;
}
bool p3p::align(double M_end[3][3],
double X0, double Y0, double Z0,
double X1, double Y1, double Z1,
double X2, double Y2, double Z2,
double R[3][3], double T[3])
{
// Centroids:
double C_start[3] = {}, C_end[3] = {};
for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
C_start[0] = (X0 + X1 + X2) / 3;
C_start[1] = (Y0 + Y1 + Y2) / 3;
C_start[2] = (Z0 + Z1 + Z2) / 3;
// Covariance matrix s:
double s[3 * 3] = {};
for(int j = 0; j < 3; j++) {
s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
}
double Qs[16] = {}, evs[4] = {}, U[16] = {};
Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
jacobi_4x4(Qs, evs, U);
// Looking for the largest eigen value:
int i_ev = 0;
double ev_max = evs[i_ev];
for(int i = 1; i < 4; i++)
if (evs[i] > ev_max)
ev_max = evs[i_ev = i];
// Quaternion:
double q[4];
for(int i = 0; i < 4; i++)
q[i] = U[i * 4 + i_ev];
double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];
double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
double q2_3 = q[2] * q[3];
R[0][0] = q02 + q12 - q22 - q32;
R[0][1] = 2. * (q1_2 - q0_3);
R[0][2] = 2. * (q1_3 + q0_2);
R[1][0] = 2. * (q1_2 + q0_3);
R[1][1] = q02 + q22 - q12 - q32;
R[1][2] = 2. * (q2_3 - q0_1);
R[2][0] = 2. * (q1_3 - q0_2);
R[2][1] = 2. * (q2_3 + q0_1);
R[2][2] = q02 + q32 - q12 - q22;
for(int i = 0; i < 3; i++)
T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);
return true;
}
bool p3p::jacobi_4x4(double * A, double * D, double * U)
{
double B[4] = {}, Z[4] = {};
double Id[16] = {1., 0., 0., 0.,
0., 1., 0., 0.,
0., 0., 1., 0.,
0., 0., 0., 1.};
memcpy(U, Id, 16 * sizeof(double));
B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
memcpy(D, B, 4 * sizeof(double));
for(int iter = 0; iter < 50; iter++) {
double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
if (sum == 0.0)
return true;
double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
for(int i = 0; i < 3; i++) {
double * pAij = A + 5 * i + 1;
for(int j = i + 1 ; j < 4; j++) {
double Aij = *pAij;
double eps_machine = 100.0 * fabs(Aij);
if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) )
*pAij = 0.0;
else if (fabs(Aij) > tresh) {
double hh = D[j] - D[i], t;
if (fabs(hh) + eps_machine == fabs(hh))
t = Aij / hh;
else {
double theta = 0.5 * hh / Aij;
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
if (theta < 0.0) t = -t;
}
hh = t * Aij;
Z[i] -= hh;
Z[j] += hh;
D[i] -= hh;
D[j] += hh;
*pAij = 0.0;
double c = 1.0 / sqrt(1 + t * t);
double s = t * c;
double tau = s / (1.0 + c);
for(int k = 0; k <= i - 1; k++) {
double g = A[k * 4 + i], h = A[k * 4 + j];
A[k * 4 + i] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = i + 1; k <= j - 1; k++) {
double g = A[i * 4 + k], h = A[k * 4 + j];
A[i * 4 + k] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = j + 1; k < 4; k++) {
double g = A[i * 4 + k], h = A[j * 4 + k];
A[i * 4 + k] = g - s * (h + g * tau);
A[j * 4 + k] = h + s * (g - h * tau);
}
for(int k = 0; k < 4; k++) {
double g = U[k * 4 + i], h = U[k * 4 + j];
U[k * 4 + i] = g - s * (h + g * tau);
U[k * 4 + j] = h + s * (g - h * tau);
}
}
pAij++;
}
}
for(int i = 0; i < 4; i++) B[i] += Z[i];
memcpy(D, B, 4 * sizeof(double));
memset(Z, 0, 4 * sizeof(double));
}
return false;
}

71
Lib/opencv/sources/modules/calib3d/src/p3p.h Normal file
View File

@@ -0,0 +1,71 @@
#ifndef P3P_H
#define P3P_H
#include "precomp.hpp"
class p3p
{
public:
p3p(double fx, double fy, double cx, double cy);
p3p(cv::Mat cameraMatrix);
bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints);
int 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);
bool solve(double R[3][3], double t[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);
private:
template <typename T>
void init_camera_parameters(const cv::Mat& cameraMatrix)
{
cx = cameraMatrix.at<T> (0, 2);
cy = cameraMatrix.at<T> (1, 2);
fx = cameraMatrix.at<T> (0, 0);
fy = cameraMatrix.at<T> (1, 1);
}
template <typename OpointType, typename IpointType>
void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
{
points.clear();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*4); //resize vector to fit for p4p case
for(int i = 0; i < npoints; i++)
{
points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
points[i*5+2] = opoints.at<OpointType>(i).x;
points[i*5+3] = opoints.at<OpointType>(i).y;
points[i*5+4] = opoints.at<OpointType>(i).z;
}
//Fill vectors with unused values for p3p case
for (int i = npoints; i < 4; i++) {
for (int j = 0; j < 5; j++) {
points[i * 5 + j] = 0;
}
}
}
void init_inverse_parameters();
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]);
bool align(double M_start[3][3],
double X0, double Y0, double Z0,
double X1, double Y1, double Z1,
double X2, double Y2, double Z2,
double R[3][3], double T[3]);
bool jacobi_4x4(double * A, double * D, double * U);
double fx, fy, cx, cy;
double inv_fx, inv_fy, cx_fx, cy_fy;
};
#endif // P3P_H

170
Lib/opencv/sources/modules/calib3d/src/polynom_solver.cpp Normal file
View File

@@ -0,0 +1,170 @@
#include "precomp.hpp"
#include "polynom_solver.h"
#include <math.h>
#include <iostream>
int solve_deg2(double a, double b, double c, double & x1, double & x2)
{
double delta = b * b - 4 * a * c;
if (delta < 0) return 0;
double inv_2a = 0.5 / a;
if (delta == 0) {
x1 = -b * inv_2a;
x2 = x1;
return 1;
}
double sqrt_delta = sqrt(delta);
x1 = (-b + sqrt_delta) * inv_2a;
x2 = (-b - sqrt_delta) * inv_2a;
return 2;
}
/// Reference : Eric W. Weisstein. "Cubic Equation." From MathWorld--A Wolfram Web Resource.
/// http://mathworld.wolfram.com/CubicEquation.html
/// \return Number of real roots found.
int solve_deg3(double a, double b, double c, double d,
double & x0, double & x1, double & x2)
{
if (a == 0) {
// Solve second order system
if (b == 0) {
// Solve first order system
if (c == 0)
return 0;
x0 = -d / c;
return 1;
}
x2 = 0;
return solve_deg2(b, c, d, x0, x1);
}
// Calculate the normalized form x^3 + a2 * x^2 + a1 * x + a0 = 0
double inv_a = 1. / a;
double b_a = inv_a * b, b_a2 = b_a * b_a;
double c_a = inv_a * c;
double d_a = inv_a * d;
// Solve the cubic equation
double Q = (3 * c_a - b_a2) / 9;
double R = (9 * b_a * c_a - 27 * d_a - 2 * b_a * b_a2) / 54;
double Q3 = Q * Q * Q;
double D = Q3 + R * R;
double b_a_3 = (1. / 3.) * b_a;
if (Q == 0) {
if(R == 0) {
x0 = x1 = x2 = - b_a_3;
return 3;
}
else {
x0 = pow(2 * R, 1 / 3.0) - b_a_3;
return 1;
}
}
if (D <= 0) {
// Three real roots
double theta = acos(R / sqrt(-Q3));
double sqrt_Q = sqrt(-Q);
x0 = 2 * sqrt_Q * cos(theta / 3.0) - b_a_3;
x1 = 2 * sqrt_Q * cos((theta + 2 * CV_PI)/ 3.0) - b_a_3;
x2 = 2 * sqrt_Q * cos((theta + 4 * CV_PI)/ 3.0) - b_a_3;
return 3;
}
// D > 0, only one real root
double AD = pow(fabs(R) + sqrt(D), 1.0 / 3.0) * (R > 0 ? 1 : (R < 0 ? -1 : 0));
double BD = (AD == 0) ? 0 : -Q / AD;
// Calculate the only real root
x0 = AD + BD - b_a_3;
return 1;
}
/// Reference : Eric W. Weisstein. "Quartic Equation." From MathWorld--A Wolfram Web Resource.
/// http://mathworld.wolfram.com/QuarticEquation.html
/// \return Number of real roots found.
int solve_deg4(double a, double b, double c, double d, double e,
double & x0, double & x1, double & x2, double & x3)
{
if (a == 0) {
x3 = 0;
return solve_deg3(b, c, d, e, x0, x1, x2);
}
// Normalize coefficients
double inv_a = 1. / a;
b *= inv_a; c *= inv_a; d *= inv_a; e *= inv_a;
double b2 = b * b, bc = b * c, b3 = b2 * b;
// Solve resultant cubic
double r0, r1, r2;
int n = solve_deg3(1, -c, d * b - 4 * e, 4 * c * e - d * d - b2 * e, r0, r1, r2);
if (n == 0) return 0;
// Calculate R^2
double R2 = 0.25 * b2 - c + r0, R;
if (R2 < 0)
return 0;
R = sqrt(R2);
double inv_R = 1. / R;
int nb_real_roots = 0;
// Calculate D^2 and E^2
double D2, E2;
if (R < 10E-12) {
double temp = r0 * r0 - 4 * e;
if (temp < 0)
D2 = E2 = -1;
else {
double sqrt_temp = sqrt(temp);
D2 = 0.75 * b2 - 2 * c + 2 * sqrt_temp;
E2 = D2 - 4 * sqrt_temp;
}
}
else {
double u = 0.75 * b2 - 2 * c - R2,
v = 0.25 * inv_R * (4 * bc - 8 * d - b3);
D2 = u + v;
E2 = u - v;
}
double b_4 = 0.25 * b, R_2 = 0.5 * R;
if (D2 >= 0) {
double D = sqrt(D2);
nb_real_roots = 2;
double D_2 = 0.5 * D;
x0 = R_2 + D_2 - b_4;
x1 = x0 - D;
}
// Calculate E^2
if (E2 >= 0) {
double E = sqrt(E2);
double E_2 = 0.5 * E;
if (nb_real_roots == 0) {
x0 = - R_2 + E_2 - b_4;
x1 = x0 - E;
nb_real_roots = 2;
}
else {
x2 = - R_2 + E_2 - b_4;
x3 = x2 - E;
nb_real_roots = 4;
}
}
return nb_real_roots;
}

12
Lib/opencv/sources/modules/calib3d/src/polynom_solver.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef POLYNOM_SOLVER_H
#define POLYNOM_SOLVER_H
int solve_deg2(double a, double b, double c, double & x1, double & x2);
int solve_deg3(double a, double b, double c, double d,
double & x0, double & x1, double & x2);
int solve_deg4(double a, double b, double c, double d, double e,
double & x0, double & x1, double & x2, double & x3);
#endif // POLYNOM_SOLVER_H

360
Lib/opencv/sources/modules/calib3d/src/posit.cpp Normal file
View File

@@ -0,0 +1,360 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "calib3d_c_api.h"
/* POSIT structure */
struct CvPOSITObject
{
int N;
float* inv_matr;
float* obj_vecs;
float* img_vecs;
};
static void icvPseudoInverse3D( float *a, float *b, int n, int method );
static CvStatus icvCreatePOSITObject( CvPoint3D32f *points,
int numPoints,
CvPOSITObject **ppObject )
{
int i;
/* Compute size of required memory */
/* buffer for inverse matrix = N*3*float */
/* buffer for storing weakImagePoints = numPoints * 2 * float */
/* buffer for storing object vectors = N*3*float */
/* buffer for storing image vectors = N*2*float */
int N = numPoints - 1;
int inv_matr_size = N * 3 * sizeof( float );
int obj_vec_size = inv_matr_size;
int img_vec_size = N * 2 * sizeof( float );
CvPOSITObject *pObject;
/* check bad arguments */
if( points == NULL )
return CV_NULLPTR_ERR;
if( numPoints < 4 )
return CV_BADSIZE_ERR;
if( ppObject == NULL )
return CV_NULLPTR_ERR;
/* memory allocation */
pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) +
inv_matr_size + obj_vec_size + img_vec_size );
if( !pObject )
return CV_OUTOFMEM_ERR;
/* part the memory between all structures */
pObject->N = N;
pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject ));
pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size);
pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size);
/****************************************************************************************\
* Construct object vectors from object points *
\****************************************************************************************/
for( i = 0; i < numPoints - 1; i++ )
{
pObject->obj_vecs[i] = points[i + 1].x - points[0].x;
pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y;
pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z;
}
/****************************************************************************************\
* Compute pseudoinverse matrix *
\****************************************************************************************/
icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 );
*ppObject = pObject;
return CV_NO_ERR;
}
static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
float focalLength, CvTermCriteria criteria,
float* rotation, float* translation )
{
int i, j, k;
int count = 0;
bool converged = false;
float scale = 0, inv_Z = 0;
float diff = (float)criteria.epsilon;
/* Check bad arguments */
if( imagePoints == NULL )
return CV_NULLPTR_ERR;
if( pObject == NULL )
return CV_NULLPTR_ERR;
if( focalLength <= 0 )
return CV_BADFACTOR_ERR;
if( !rotation )
return CV_NULLPTR_ERR;
if( !translation )
return CV_NULLPTR_ERR;
if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS)))
return CV_BADFLAG_ERR;
if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 )
return CV_BADFACTOR_ERR;
if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 )
return CV_BADFACTOR_ERR;
/* init variables */
float inv_focalLength = 1 / focalLength;
int N = pObject->N;
float *objectVectors = pObject->obj_vecs;
float *invMatrix = pObject->inv_matr;
float *imgVectors = pObject->img_vecs;
while( !converged )
{
if( count == 0 )
{
/* subtract out origin to get image vectors */
for( i = 0; i < N; i++ )
{
imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x;
imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y;
}
}
else
{
diff = 0;
/* Compute new SOP (scaled orthograthic projection) image from pose */
for( i = 0; i < N; i++ )
{
/* objectVector * k */
float old;
float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ +
objectVectors[N + i] * rotation[7] /*[2][1]*/ +
objectVectors[2 * N + i] * rotation[8] /*[2][2]*/;
tmp *= inv_Z;
tmp += 1;
old = imgVectors[i];
imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x;
diff = MAX( diff, (float) fabs( imgVectors[i] - old ));
old = imgVectors[N + i];
imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y;
diff = MAX( diff, (float) fabs( imgVectors[N + i] - old ));
}
}
/* calculate I and J vectors */
for( i = 0; i < 2; i++ )
{
for( j = 0; j < 3; j++ )
{
rotation[3*i+j] /*[i][j]*/ = 0;
for( k = 0; k < N; k++ )
{
rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k];
}
}
}
float inorm =
rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ +
rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/;
float jnorm =
rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ +
rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/;
const float invInorm = cvInvSqrt( inorm );
const float invJnorm = cvInvSqrt( jnorm );
inorm *= invInorm;
jnorm *= invJnorm;
rotation[0] /*[0][0]*/ *= invInorm;
rotation[1] /*[0][1]*/ *= invInorm;
rotation[2] /*[0][2]*/ *= invInorm;
rotation[3] /*[1][0]*/ *= invJnorm;
rotation[4] /*[1][1]*/ *= invJnorm;
rotation[5] /*[1][2]*/ *= invJnorm;
/* row2 = row0 x row1 (cross product) */
rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ -
rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/;
rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ -
rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/;
rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ -
rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/;
scale = (inorm + jnorm) / 2.0f;
inv_Z = scale * inv_focalLength;
count++;
converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon))
|| ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
}
const float invScale = 1 / scale;
translation[0] = imagePoints[0].x * invScale;
translation[1] = imagePoints[0].y * invScale;
translation[2] = 1 / inv_Z;
return CV_NO_ERR;
}
static CvStatus icvReleasePOSITObject( CvPOSITObject ** ppObject )
{
cvFree( ppObject );
return CV_NO_ERR;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvPseudoInverse3D
// Purpose: Pseudoinverse N x 3 matrix N >= 3
// Context:
// Parameters:
// a - input matrix
// b - pseudoinversed a
// n - number of rows in a
// method - if 0, then b = inv(transpose(a)*a) * transpose(a)
// if 1, then SVD used.
// Returns:
// Notes: Both matrix are stored by n-dimensional vectors.
// Now only method == 0 supported.
//F*/
void
icvPseudoInverse3D( float *a, float *b, int n, int method )
{
if( method == 0 )
{
float ata00 = 0;
float ata11 = 0;
float ata22 = 0;
float ata01 = 0;
float ata02 = 0;
float ata12 = 0;
int k;
/* compute matrix ata = transpose(a) * a */
for( k = 0; k < n; k++ )
{
float a0 = a[k];
float a1 = a[n + k];
float a2 = a[2 * n + k];
ata00 += a0 * a0;
ata11 += a1 * a1;
ata22 += a2 * a2;
ata01 += a0 * a1;
ata02 += a0 * a2;
ata12 += a1 * a2;
}
/* inverse matrix ata */
{
float p00 = ata11 * ata22 - ata12 * ata12;
float p01 = -(ata01 * ata22 - ata12 * ata02);
float p02 = ata12 * ata01 - ata11 * ata02;
float p11 = ata00 * ata22 - ata02 * ata02;
float p12 = -(ata00 * ata12 - ata01 * ata02);
float p22 = ata00 * ata11 - ata01 * ata01;
float det = 0;
det += ata00 * p00;
det += ata01 * p01;
det += ata02 * p02;
const float inv_det = 1 / det;
/* compute resultant matrix */
for( k = 0; k < n; k++ )
{
float a0 = a[k];
float a1 = a[n + k];
float a2 = a[2 * n + k];
b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det;
b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det;
b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det;
}
}
}
/*if ( method == 1 )
{
}
*/
return;
}
CV_IMPL CvPOSITObject *
cvCreatePOSITObject( CvPoint3D32f * points, int numPoints )
{
CvPOSITObject *pObject = 0;
IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject ));
return pObject;
}
CV_IMPL void
cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints,
double focalLength, CvTermCriteria criteria,
float* rotation, float* translation )
{
IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria,
rotation, translation ));
}
CV_IMPL void
cvReleasePOSITObject( CvPOSITObject ** ppObject )
{
IPPI_CALL( icvReleasePOSITObject( ppObject ));
}
/* End of file. */

142
Lib/opencv/sources/modules/calib3d/src/precomp.hpp Normal file
View File

@@ -0,0 +1,142 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__
#include "opencv2/core/utility.hpp"
#include "opencv2/core/private.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/core/ocl.hpp"
#define GET_OPTIMIZED(func) (func)
namespace cv
{
/**
* Compute the number of iterations given the confidence, outlier ratio, number
* of model points and the maximum iteration number.
*
* @param p confidence value
* @param ep outlier ratio
* @param modelPoints number of model points required for estimation
* @param maxIters maximum number of iterations
* @return
* \f[
* \frac{\ln(1-p)}{\ln\left(1-(1-ep)^\mathrm{modelPoints}\right)}
* \f]
*
* If the computed number of iterations is larger than maxIters, then maxIters is returned.
*/
int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters );
class CV_EXPORTS PointSetRegistrator : public Algorithm
{
public:
class CV_EXPORTS Callback
{
public:
virtual ~Callback() {}
virtual int runKernel(InputArray m1, InputArray m2, OutputArray model) const = 0;
virtual void computeError(InputArray m1, InputArray m2, InputArray model, OutputArray err) const = 0;
virtual bool checkSubset(InputArray, InputArray, int) const { return true; }
};
virtual void setCallback(const Ptr<PointSetRegistrator::Callback>& cb) = 0;
virtual bool run(InputArray m1, InputArray m2, OutputArray model, OutputArray mask) const = 0;
};
CV_EXPORTS Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
int modelPoints, double threshold,
double confidence=0.99, int maxIters=1000 );
CV_EXPORTS Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
int modelPoints, double confidence=0.99, int maxIters=1000 );
template<typename T> inline int compressElems( T* ptr, const uchar* mask, int mstep, int count )
{
int i, j;
for( i = j = 0; i < count; i++ )
if( mask[i*mstep] )
{
if( i > j )
ptr[j] = ptr[i];
j++;
}
return j;
}
static inline bool haveCollinearPoints( const Mat& m, int count )
{
int j, k, i = count-1;
const Point2f* ptr = m.ptr<Point2f>();
// check that the i-th selected point does not belong
// to a line connecting some previously selected points
// also checks that points are not too close to each other
for( j = 0; j < i; j++ )
{
double dx1 = ptr[j].x - ptr[i].x;
double dy1 = ptr[j].y - ptr[i].y;
for( k = 0; k < j; k++ )
{
double dx2 = ptr[k].x - ptr[i].x;
double dy2 = ptr[k].y - ptr[i].y;
if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
return true;
}
}
return false;
}
} // namespace cv
int checkChessboardBinary(const cv::Mat & img, const cv::Size & size);
#endif

982
Lib/opencv/sources/modules/calib3d/src/ptsetreg.cpp Normal file
View File

@@ -0,0 +1,982 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <algorithm>
#include <iterator>
#include <limits>
namespace cv
{
int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
{
if( modelPoints <= 0 )
CV_Error( Error::StsOutOfRange, "the number of model points should be positive" );
p = MAX(p, 0.);
p = MIN(p, 1.);
ep = MAX(ep, 0.);
ep = MIN(ep, 1.);
// avoid inf's & nan's
double num = MAX(1. - p, DBL_MIN);
double denom = 1. - std::pow(1. - ep, modelPoints);
if( denom < DBL_MIN )
return 0;
num = std::log(num);
denom = std::log(denom);
return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
}
class RANSACPointSetRegistrator : public PointSetRegistrator
{
public:
RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
: cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters) {}
int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
{
cb->computeError( m1, m2, model, err );
mask.create(err.size(), CV_8U);
CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
const float* errptr = err.ptr<float>();
uchar* maskptr = mask.ptr<uchar>();
float t = (float)(thresh*thresh);
int i, n = (int)err.total(), nz = 0;
for( i = 0; i < n; i++ )
{
int f = errptr[i] <= t;
maskptr[i] = (uchar)f;
nz += f;
}
return nz;
}
bool getSubset( const Mat& m1, const Mat& m2, Mat& ms1, Mat& ms2, RNG& rng, int maxAttempts=1000 ) const
{
cv::AutoBuffer<int> _idx(modelPoints);
int* idx = _idx.data();
const int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
const int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
int esz1 = (int)m1.elemSize1() * d1;
int esz2 = (int)m2.elemSize1() * d2;
CV_Assert((esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0);
esz1 /= sizeof(int);
esz2 /= sizeof(int);
const int count = m1.checkVector(d1);
const int count2 = m2.checkVector(d2);
CV_Assert(count >= modelPoints && count == count2);
const int *m1ptr = m1.ptr<int>();
const int *m2ptr = m2.ptr<int>();
ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));
int *ms1ptr = ms1.ptr<int>();
int *ms2ptr = ms2.ptr<int>();
for( int iters = 0; iters < maxAttempts; ++iters )
{
int i;
for( i = 0; i < modelPoints; ++i )
{
int idx_i;
for ( idx_i = rng.uniform(0, count);
std::find(idx, idx + i, idx_i) != idx + i;
idx_i = rng.uniform(0, count) )
{}
idx[i] = idx_i;
for( int k = 0; k < esz1; ++k )
ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
for( int k = 0; k < esz2; ++k )
ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
}
if( cb->checkSubset(ms1, ms2, i) )
return true;
}
return false;
}
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const CV_OVERRIDE
{
bool result = false;
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
Mat err, mask, model, bestModel, ms1, ms2;
int iter, niters = MAX(maxIters, 1);
int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;
RNG rng((uint64)-1);
CV_Assert( cb );
CV_Assert( confidence > 0 && confidence < 1 );
CV_Assert( count >= 0 && count2 == count );
if( count < modelPoints )
return false;
Mat bestMask0, bestMask;
if( _mask.needed() )
{
_mask.create(count, 1, CV_8U, -1, true);
bestMask0 = bestMask = _mask.getMat();
CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
}
else
{
bestMask.create(count, 1, CV_8U);
bestMask0 = bestMask;
}
if( count == modelPoints )
{
if( cb->runKernel(m1, m2, bestModel) <= 0 )
return false;
bestModel.copyTo(_model);
bestMask.setTo(Scalar::all(1));
return true;
}
for( iter = 0; iter < niters; iter++ )
{
int i, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
if( !found )
{
if( iter == 0 )
return false;
break;
}
}
nmodels = cb->runKernel( ms1, ms2, model );
if( nmodels <= 0 )
continue;
CV_Assert( model.rows % nmodels == 0 );
Size modelSize(model.cols, model.rows/nmodels);
for( i = 0; i < nmodels; i++ )
{
Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
int goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
if( goodCount > MAX(maxGoodCount, modelPoints-1) )
{
std::swap(mask, bestMask);
model_i.copyTo(bestModel);
maxGoodCount = goodCount;
niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
}
}
}
if( maxGoodCount > 0 )
{
if( bestMask.data != bestMask0.data )
{
if( bestMask.size() == bestMask0.size() )
bestMask.copyTo(bestMask0);
else
transpose(bestMask, bestMask0);
}
bestModel.copyTo(_model);
result = true;
}
else
_model.release();
return result;
}
void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) CV_OVERRIDE { cb = _cb; }
Ptr<PointSetRegistrator::Callback> cb;
int modelPoints;
double threshold;
double confidence;
int maxIters;
};
class LMeDSPointSetRegistrator : public RANSACPointSetRegistrator
{
public:
LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
: RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const CV_OVERRIDE
{
const double outlierRatio = 0.45;
bool result = false;
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
Mat ms1, ms2, err, errf, model, bestModel, mask, mask0;
int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
double minMedian = DBL_MAX;
RNG rng((uint64)-1);
CV_Assert( cb );
CV_Assert( confidence > 0 && confidence < 1 );
CV_Assert( count >= 0 && count2 == count );
if( count < modelPoints )
return false;
if( _mask.needed() )
{
_mask.create(count, 1, CV_8U, -1, true);
mask0 = mask = _mask.getMat();
CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count );
}
if( count == modelPoints )
{
if( cb->runKernel(m1, m2, bestModel) <= 0 )
return false;
bestModel.copyTo(_model);
mask.setTo(Scalar::all(1));
return true;
}
int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters);
niters = MAX(niters, 3);
for( iter = 0; iter < niters; iter++ )
{
int i, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, rng );
if( !found )
{
if( iter == 0 )
return false;
break;
}
}
nmodels = cb->runKernel( ms1, ms2, model );
if( nmodels <= 0 )
continue;
CV_Assert( model.rows % nmodels == 0 );
Size modelSize(model.cols, model.rows/nmodels);
for( i = 0; i < nmodels; i++ )
{
Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
cb->computeError( m1, m2, model_i, err );
if( err.depth() != CV_32F )
err.convertTo(errf, CV_32F);
else
errf = err;
CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
std::nth_element(errf.ptr<int>(), errf.ptr<int>() + count/2, errf.ptr<int>() + count);
double median = errf.at<float>(count/2);
if( median < minMedian )
{
minMedian = median;
model_i.copyTo(bestModel);
}
}
}
if( minMedian < DBL_MAX )
{
double sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
sigma = MAX( sigma, 0.001 );
count = findInliers( m1, m2, bestModel, err, mask, sigma );
if( _mask.needed() && mask0.data != mask.data )
{
if( mask0.size() == mask.size() )
mask.copyTo(mask0);
else
transpose(mask, mask0);
}
bestModel.copyTo(_model);
result = count >= modelPoints;
}
else
_model.release();
return result;
}
};
Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
int _modelPoints, double _threshold,
double _confidence, int _maxIters)
{
return Ptr<PointSetRegistrator>(
new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
}
Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
int _modelPoints, double _confidence, int _maxIters)
{
return Ptr<PointSetRegistrator>(
new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters));
}
/*
* Compute
* x a b c X t1
* y = d e f * Y + t2
* z g h i Z t3
*
* - every element in _m1 contains (X,Y,Z), which are called source points
* - every element in _m2 contains (x,y,z), which are called destination points
* - _model is of size 3x4, which contains
* a b c t1
* d e f t2
* g h i t3
*/
class Affine3DEstimatorCallback : public PointSetRegistrator::Callback
{
public:
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
{
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
const Point3f* from = m1.ptr<Point3f>();
const Point3f* to = m2.ptr<Point3f>();
const int N = 12;
double buf[N*N + N + N];
Mat A(N, N, CV_64F, &buf[0]);
Mat B(N, 1, CV_64F, &buf[0] + N*N);
Mat X(N, 1, CV_64F, &buf[0] + N*N + N);
double* Adata = A.ptr<double>();
double* Bdata = B.ptr<double>();
A = Scalar::all(0);
for( int i = 0; i < (N/3); i++ )
{
Bdata[i*3] = to[i].x;
Bdata[i*3+1] = to[i].y;
Bdata[i*3+2] = to[i].z;
double *aptr = Adata + i*3*N;
for(int k = 0; k < 3; ++k)
{
aptr[0] = from[i].x;
aptr[1] = from[i].y;
aptr[2] = from[i].z;
aptr[3] = 1.0;
aptr += 16;
}
}
solve(A, B, X, DECOMP_SVD);
X.reshape(1, 3).copyTo(_model);
return 1;
}
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
{
Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
const Point3f* from = m1.ptr<Point3f>();
const Point3f* to = m2.ptr<Point3f>();
const double* F = model.ptr<double>();
int count = m1.checkVector(3);
CV_Assert( count > 0 );
_err.create(count, 1, CV_32F);
Mat err = _err.getMat();
float* errptr = err.ptr<float>();
for(int i = 0; i < count; i++ )
{
const Point3f& f = from[i];
const Point3f& t = to[i];
double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
errptr[i] = (float)(a*a + b*b + c*c);
}
}
bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
{
const float threshold = 0.996f;
Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
for( int inp = 1; inp <= 2; inp++ )
{
int j, k, i = count - 1;
const Mat* msi = inp == 1 ? &ms1 : &ms2;
const Point3f* ptr = msi->ptr<Point3f>();
CV_Assert( count <= msi->rows );
// check that the i-th selected point does not belong
// to a line connecting some previously selected points
for(j = 0; j < i; ++j)
{
Point3f d1 = ptr[j] - ptr[i];
float n1 = d1.x*d1.x + d1.y*d1.y + d1.z*d1.z;
for(k = 0; k < j; ++k)
{
Point3f d2 = ptr[k] - ptr[i];
float denom = (d2.x*d2.x + d2.y*d2.y + d2.z*d2.z)*n1;
float num = d1.x*d2.x + d1.y*d2.y + d1.z*d2.z;
if( num*num > threshold*threshold*denom )
return false;
}
}
}
return true;
}
};
/*
* Compute
* x a b X c
* = * +
* y d e Y f
*
* - every element in _m1 contains (X,Y), which are called source points
* - every element in _m2 contains (x,y), which are called destination points
* - _model is of size 2x3, which contains
* a b c
* d e f
*/
class Affine2DEstimatorCallback : public PointSetRegistrator::Callback
{
public:
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
{
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
const Point2f* from = m1.ptr<Point2f>();
const Point2f* to = m2.ptr<Point2f>();
_model.create(2, 3, CV_64F);
Mat M_mat = _model.getMat();
double *M = M_mat.ptr<double>();
// we need 3 points to estimate affine transform
double x1 = from[0].x;
double y1 = from[0].y;
double x2 = from[1].x;
double y2 = from[1].y;
double x3 = from[2].x;
double y3 = from[2].y;
double X1 = to[0].x;
double Y1 = to[0].y;
double X2 = to[1].x;
double Y2 = to[1].y;
double X3 = to[2].x;
double Y3 = to[2].y;
/*
We want to solve AX = B
| x1 y1 1 0 0 0 |
| 0 0 0 x1 y1 1 |
| x2 y2 1 0 0 0 |
A = | 0 0 0 x2 y2 1 |
| x3 y3 1 0 0 0 |
| 0 0 0 x3 y3 1 |
B = (X1, Y1, X2, Y2, X3, Y3).t()
X = (a, b, c, d, e, f).t()
As the estimate of (a, b, c) only depends on the Xi, and (d, e, f) only
depends on the Yi, we do the *trick* to solve each one analytically.
| X1 | | x1 y1 1 | | a |
| X2 | = | x2 y2 1 | * | b |
| X3 | | x3 y3 1 | | c |
| Y1 | | x1 y1 1 | | d |
| Y2 | = | x2 y2 1 | * | e |
| Y3 | | x3 y3 1 | | f |
*/
double d = 1. / ( x1*(y2-y3) + x2*(y3-y1) + x3*(y1-y2) );
M[0] = d * ( X1*(y2-y3) + X2*(y3-y1) + X3*(y1-y2) );
M[1] = d * ( X1*(x3-x2) + X2*(x1-x3) + X3*(x2-x1) );
M[2] = d * ( X1*(x2*y3 - x3*y2) + X2*(x3*y1 - x1*y3) + X3*(x1*y2 - x2*y1) );
M[3] = d * ( Y1*(y2-y3) + Y2*(y3-y1) + Y3*(y1-y2) );
M[4] = d * ( Y1*(x3-x2) + Y2*(x1-x3) + Y3*(x2-x1) );
M[5] = d * ( Y1*(x2*y3 - x3*y2) + Y2*(x3*y1 - x1*y3) + Y3*(x1*y2 - x2*y1) );
return 1;
}
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
{
Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
const Point2f* from = m1.ptr<Point2f>();
const Point2f* to = m2.ptr<Point2f>();
const double* F = model.ptr<double>();
int count = m1.checkVector(2);
CV_Assert( count > 0 );
_err.create(count, 1, CV_32F);
Mat err = _err.getMat();
float* errptr = err.ptr<float>();
// transform matrix to floats
float F0 = (float)F[0], F1 = (float)F[1], F2 = (float)F[2];
float F3 = (float)F[3], F4 = (float)F[4], F5 = (float)F[5];
for(int i = 0; i < count; i++ )
{
const Point2f& f = from[i];
const Point2f& t = to[i];
float a = F0*f.x + F1*f.y + F2 - t.x;
float b = F3*f.x + F4*f.y + F5 - t.y;
errptr[i] = a*a + b*b;
}
}
bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const CV_OVERRIDE
{
Mat ms1 = _ms1.getMat();
Mat ms2 = _ms2.getMat();
// check collinearity and also check that points are too close
return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count);
}
};
/*
* Compute
* x c -s X t1
* = * +
* y s c Y t2
*
* - every element in _m1 contains (X,Y), which are called source points
* - every element in _m2 contains (x,y), which are called destination points
* - _model is of size 2x3, which contains
* c -s t1
* s c t2
*/
class AffinePartial2DEstimatorCallback : public Affine2DEstimatorCallback
{
public:
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
{
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
const Point2f* from = m1.ptr<Point2f>();
const Point2f* to = m2.ptr<Point2f>();
_model.create(2, 3, CV_64F);
Mat M_mat = _model.getMat();
double *M = M_mat.ptr<double>();
// we need only 2 points to estimate transform
double x1 = from[0].x;
double y1 = from[0].y;
double x2 = from[1].x;
double y2 = from[1].y;
double X1 = to[0].x;
double Y1 = to[0].y;
double X2 = to[1].x;
double Y2 = to[1].y;
/*
we are solving AS = B
| x1 -y1 1 0 |
| y1 x1 0 1 |
A = | x2 -y2 1 0 |
| y2 x2 0 1 |
B = (X1, Y1, X2, Y2).t()
we solve that analytically
*/
double d = 1./((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
// solution vector
double S0 = d * ( (X1-X2)*(x1-x2) + (Y1-Y2)*(y1-y2) );
double S1 = d * ( (Y1-Y2)*(x1-x2) - (X1-X2)*(y1-y2) );
double S2 = d * ( (Y1-Y2)*(x1*y2 - x2*y1) - (X1*y2 - X2*y1)*(y1-y2) - (X1*x2 - X2*x1)*(x1-x2) );
double S3 = d * (-(X1-X2)*(x1*y2 - x2*y1) - (Y1*x2 - Y2*x1)*(x1-x2) - (Y1*y2 - Y2*y1)*(y1-y2) );
// set model, rotation part is antisymmetric
M[0] = M[4] = S0;
M[1] = -S1;
M[2] = S2;
M[3] = S1;
M[5] = S3;
return 1;
}
};
class Affine2DRefineCallback : public LMSolver::Callback
{
public:
Affine2DRefineCallback(InputArray _src, InputArray _dst)
{
src = _src.getMat();
dst = _dst.getMat();
}
bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
{
int i, count = src.checkVector(2);
Mat param = _param.getMat();
_err.create(count*2, 1, CV_64F);
Mat err = _err.getMat(), J;
if( _Jac.needed())
{
_Jac.create(count*2, param.rows, CV_64F);
J = _Jac.getMat();
CV_Assert( J.isContinuous() && J.cols == 6 );
}
const Point2f* M = src.ptr<Point2f>();
const Point2f* m = dst.ptr<Point2f>();
const double* h = param.ptr<double>();
double* errptr = err.ptr<double>();
double* Jptr = J.data ? J.ptr<double>() : 0;
for( i = 0; i < count; i++ )
{
double Mx = M[i].x, My = M[i].y;
double xi = h[0]*Mx + h[1]*My + h[2];
double yi = h[3]*Mx + h[4]*My + h[5];
errptr[i*2] = xi - m[i].x;
errptr[i*2+1] = yi - m[i].y;
/*
Jacobian should be:
{x, y, 1, 0, 0, 0}
{0, 0, 0, x, y, 1}
*/
if( Jptr )
{
Jptr[0] = Mx; Jptr[1] = My; Jptr[2] = 1.;
Jptr[3] = Jptr[4] = Jptr[5] = 0.;
Jptr[6] = Jptr[7] = Jptr[8] = 0.;
Jptr[9] = Mx; Jptr[10] = My; Jptr[11] = 1.;
Jptr += 6*2;
}
}
return true;
}
Mat src, dst;
};
class AffinePartial2DRefineCallback : public LMSolver::Callback
{
public:
AffinePartial2DRefineCallback(InputArray _src, InputArray _dst)
{
src = _src.getMat();
dst = _dst.getMat();
}
bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
{
int i, count = src.checkVector(2);
Mat param = _param.getMat();
_err.create(count*2, 1, CV_64F);
Mat err = _err.getMat(), J;
if( _Jac.needed())
{
_Jac.create(count*2, param.rows, CV_64F);
J = _Jac.getMat();
CV_Assert( J.isContinuous() && J.cols == 4 );
}
const Point2f* M = src.ptr<Point2f>();
const Point2f* m = dst.ptr<Point2f>();
const double* h = param.ptr<double>();
double* errptr = err.ptr<double>();
double* Jptr = J.data ? J.ptr<double>() : 0;
for( i = 0; i < count; i++ )
{
double Mx = M[i].x, My = M[i].y;
double xi = h[0]*Mx - h[1]*My + h[2];
double yi = h[1]*Mx + h[0]*My + h[3];
errptr[i*2] = xi - m[i].x;
errptr[i*2+1] = yi - m[i].y;
/*
Jacobian should be:
{x, -y, 1, 0}
{y, x, 0, 1}
*/
if( Jptr )
{
Jptr[0] = Mx; Jptr[1] = -My; Jptr[2] = 1.; Jptr[3] = 0.;
Jptr[4] = My; Jptr[5] = Mx; Jptr[6] = 0.; Jptr[7] = 1.;
Jptr += 4*2;
}
}
return true;
}
Mat src, dst;
};
int estimateAffine3D(InputArray _from, InputArray _to,
OutputArray _out, OutputArray _inliers,
double ransacThreshold, double confidence)
{
CV_INSTRUMENT_REGION();
Mat from = _from.getMat(), to = _to.getMat();
int count = from.checkVector(3);
CV_Assert( count >= 0 && to.checkVector(3) == count );
Mat dFrom, dTo;
from.convertTo(dFrom, CV_32F);
to.convertTo(dTo, CV_32F);
dFrom = dFrom.reshape(3, count);
dTo = dTo.reshape(3, count);
const double epsilon = DBL_EPSILON;
ransacThreshold = ransacThreshold <= 0 ? 3 : ransacThreshold;
confidence = (confidence < epsilon) ? 0.99 : (confidence > 1 - epsilon) ? 0.99 : confidence;
return createRANSACPointSetRegistrator(makePtr<Affine3DEstimatorCallback>(), 4, ransacThreshold, confidence)->run(dFrom, dTo, _out, _inliers);
}
Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
const int method, const double ransacReprojThreshold,
const size_t maxIters, const double confidence,
const size_t refineIters)
{
Mat from = _from.getMat(), to = _to.getMat();
int count = from.checkVector(2);
bool result = false;
Mat H;
CV_Assert( count >= 0 && to.checkVector(2) == count );
if (from.type() != CV_32FC2 || to.type() != CV_32FC2)
{
Mat tmp1, tmp2;
from.convertTo(tmp1, CV_32FC2);
from = tmp1;
to.convertTo(tmp2, CV_32FC2);
to = tmp2;
}
else
{
// avoid changing of inputs in compressElems() call
from = from.clone();
to = to.clone();
}
// convert to N x 1 vectors
from = from.reshape(2, count);
to = to.reshape(2, count);
Mat inliers;
if(_inliers.needed())
{
_inliers.create(count, 1, CV_8U, -1, true);
inliers = _inliers.getMat();
}
// run robust method
Ptr<PointSetRegistrator::Callback> cb = makePtr<Affine2DEstimatorCallback>();
if( method == RANSAC )
result = createRANSACPointSetRegistrator(cb, 3, ransacReprojThreshold, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
else if( method == LMEDS )
result = createLMeDSPointSetRegistrator(cb, 3, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
else
CV_Error(Error::StsBadArg, "Unknown or unsupported robust estimation method");
if(result && count > 3 && refineIters)
{
// reorder to start with inliers
compressElems(from.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
int inliers_count = compressElems(to.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
if(inliers_count > 0)
{
Mat src = from.rowRange(0, inliers_count);
Mat dst = to.rowRange(0, inliers_count);
Mat Hvec = H.reshape(1, 6);
LMSolver::create(makePtr<Affine2DRefineCallback>(src, dst), static_cast<int>(refineIters))->run(Hvec);
}
}
if (!result)
{
H.release();
if(_inliers.needed())
{
inliers = Mat::zeros(count, 1, CV_8U);
inliers.copyTo(_inliers);
}
}
return H;
}
Mat estimateAffinePartial2D(InputArray _from, InputArray _to, OutputArray _inliers,
const int method, const double ransacReprojThreshold,
const size_t maxIters, const double confidence,
const size_t refineIters)
{
Mat from = _from.getMat(), to = _to.getMat();
const int count = from.checkVector(2);
bool result = false;
Mat H;
CV_Assert( count >= 0 && to.checkVector(2) == count );
if (from.type() != CV_32FC2 || to.type() != CV_32FC2)
{
Mat tmp1, tmp2;
from.convertTo(tmp1, CV_32FC2);
from = tmp1;
to.convertTo(tmp2, CV_32FC2);
to = tmp2;
}
else
{
// avoid changing of inputs in compressElems() call
from = from.clone();
to = to.clone();
}
// convert to N x 1 vectors
from = from.reshape(2, count);
to = to.reshape(2, count);
Mat inliers;
if(_inliers.needed())
{
_inliers.create(count, 1, CV_8U, -1, true);
inliers = _inliers.getMat();
}
// run robust estimation
Ptr<PointSetRegistrator::Callback> cb = makePtr<AffinePartial2DEstimatorCallback>();
if( method == RANSAC )
result = createRANSACPointSetRegistrator(cb, 2, ransacReprojThreshold, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
else if( method == LMEDS )
result = createLMeDSPointSetRegistrator(cb, 2, confidence, static_cast<int>(maxIters))->run(from, to, H, inliers);
else
CV_Error(Error::StsBadArg, "Unknown or unsupported robust estimation method");
if(result && count > 2 && refineIters)
{
// reorder to start with inliers
compressElems(from.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
int inliers_count = compressElems(to.ptr<Point2f>(), inliers.ptr<uchar>(), 1, count);
if(inliers_count > 0)
{
Mat src = from.rowRange(0, inliers_count);
Mat dst = to.rowRange(0, inliers_count);
// H is
// a -b tx
// b a ty
// Hvec model for LevMarq is
// (a, b, tx, ty)
double *Hptr = H.ptr<double>();
double Hvec_buf[4] = {Hptr[0], Hptr[3], Hptr[2], Hptr[5]};
Mat Hvec (4, 1, CV_64F, Hvec_buf);
LMSolver::create(makePtr<AffinePartial2DRefineCallback>(src, dst), static_cast<int>(refineIters))->run(Hvec);
// update H with refined parameters
Hptr[0] = Hptr[4] = Hvec_buf[0];
Hptr[1] = -Hvec_buf[1];
Hptr[2] = Hvec_buf[2];
Hptr[3] = Hvec_buf[1];
Hptr[5] = Hvec_buf[3];
}
}
if (!result)
{
H.release();
if(_inliers.needed())
{
inliers = Mat::zeros(count, 1, CV_8U);
inliers.copyTo(_inliers);
}
}
return H;
}
} // namespace cv

240
Lib/opencv/sources/modules/calib3d/src/quadsubpix.cpp Normal file
View File

@@ -0,0 +1,240 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <limits>
#include <utility>
#include <algorithm>
#include <math.h>
namespace cv {
inline bool is_smaller(const std::pair<int, float>& p1, const std::pair<int, float>& p2)
{
return p1.second < p2.second;
}
static void orderContours(const std::vector<std::vector<Point> >& contours, Point2f point, std::vector<std::pair<int, float> >& order)
{
order.clear();
size_t i, j, n = contours.size();
for(i = 0; i < n; i++)
{
size_t ni = contours[i].size();
double min_dist = std::numeric_limits<double>::max();
for(j = 0; j < ni; j++)
{
double dist = norm(Point2f((float)contours[i][j].x, (float)contours[i][j].y) - point);
min_dist = MIN(min_dist, dist);
}
order.push_back(std::pair<int, float>((int)i, (float)min_dist));
}
std::sort(order.begin(), order.end(), is_smaller);
}
// fit second order curve to a set of 2D points
inline void fitCurve2Order(const std::vector<Point2f>& /*points*/, std::vector<float>& /*curve*/)
{
// TBD
}
inline void findCurvesCross(const std::vector<float>& /*curve1*/, const std::vector<float>& /*curve2*/, Point2f& /*cross_point*/)
{
}
static void findLinesCrossPoint(Point2f origin1, Point2f dir1, Point2f origin2, Point2f dir2, Point2f& cross_point)
{
float det = dir2.x*dir1.y - dir2.y*dir1.x;
Point2f offset = origin2 - origin1;
float alpha = (dir2.x*offset.y - dir2.y*offset.x)/det;
cross_point = origin1 + dir1*alpha;
}
static void findCorner(const std::vector<Point2f>& contour, Point2f point, Point2f& corner)
{
// find the nearest point
double min_dist = std::numeric_limits<double>::max();
int min_idx = -1;
// find corner idx
for(size_t i = 0; i < contour.size(); i++)
{
double dist = norm(contour[i] - point);
if(dist < min_dist)
{
min_dist = dist;
min_idx = (int)i;
}
}
CV_Assert(min_idx >= 0);
// temporary solution, have to make something more precise
corner = contour[min_idx];
return;
}
static int segment_hist_max(const Mat& hist, int& low_thresh, int& high_thresh)
{
Mat bw;
double total_sum = sum(hist).val[0];
double quantile_sum = 0.0;
//double min_quantile = 0.2;
double low_sum = 0;
double max_segment_length = 0;
int max_start_x = -1;
int max_end_x = -1;
int start_x = 0;
const double out_of_bells_fraction = 0.1;
for(int x = 0; x < hist.size[0]; x++)
{
quantile_sum += hist.at<float>(x);
if(quantile_sum < 0.2*total_sum) continue;
if(quantile_sum - low_sum > out_of_bells_fraction*total_sum)
{
if(max_segment_length < x - start_x)
{
max_segment_length = x - start_x;
max_start_x = start_x;
max_end_x = x;
}
low_sum = quantile_sum;
start_x = x;
}
}
if(start_x == -1)
{
return 0;
}
else
{
low_thresh = cvRound(max_start_x + 0.25*(max_end_x - max_start_x));
high_thresh = cvRound(max_start_x + 0.75*(max_end_x - max_start_x));
return 1;
}
}
}
bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
{
CV_INSTRUMENT_REGION();
Mat img = _img.getMat(), cornersM = _corners.getMat();
int ncorners = cornersM.checkVector(2, CV_32F);
CV_Assert( ncorners >= 0 );
Point2f* corners = cornersM.ptr<Point2f>();
const int nbins = 256;
float ranges[] = {0, 256};
const float* _ranges = ranges;
Mat hist;
Mat black_comp, white_comp;
for(int i = 0; i < ncorners; i++)
{
int channels = 0;
Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height),
region_size.width*2 + 1, region_size.height*2 + 1);
Mat img_roi = img(roi);
calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges);
int black_thresh = 0, white_thresh = 0;
segment_hist_max(hist, black_thresh, white_thresh);
threshold(img, black_comp, black_thresh, 255.0, THRESH_BINARY_INV);
threshold(img, white_comp, white_thresh, 255.0, THRESH_BINARY);
const int erode_count = 1;
erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count);
erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);
std::vector<std::vector<Point> > white_contours, black_contours;
findContours(black_comp, black_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
findContours(white_comp, white_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
if(black_contours.size() < 5 || white_contours.size() < 5) continue;
// find two white and black blobs that are close to the input point
std::vector<std::pair<int, float> > white_order, black_order;
orderContours(black_contours, corners[i], black_order);
orderContours(white_contours, corners[i], white_order);
const float max_dist = 10.0f;
if(black_order[0].second > max_dist || black_order[1].second > max_dist ||
white_order[0].second > max_dist || white_order[1].second > max_dist)
{
continue; // there will be no improvement in this corner position
}
const std::vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first],
&white_contours[white_order[0].first], &white_contours[white_order[1].first]};
std::vector<Point2f> quads_approx[4];
Point2f quad_corners[4];
for(int k = 0; k < 4; k++)
{
std::vector<Point2f> temp;
for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]);
approxPolyDP(Mat(temp), quads_approx[k], 0.5, true);
findCorner(quads_approx[k], corners[i], quad_corners[k]);
quad_corners[k] += Point2f(0.5f, 0.5f);
}
// cross two lines
Point2f origin1 = quad_corners[0];
Point2f dir1 = quad_corners[1] - quad_corners[0];
Point2f origin2 = quad_corners[2];
Point2f dir2 = quad_corners[3] - quad_corners[2];
double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2)));
if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue;
findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]);
}
return true;
}

2721
Lib/opencv/sources/modules/calib3d/src/rho.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

267
Lib/opencv/sources/modules/calib3d/src/rho.h Normal file
View File

@@ -0,0 +1,267 @@
/*
IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
By downloading, copying, installing or using the software you agree to this license.
If you do not agree to this license, do not download, install,
copy or use the software.
BSD 3-Clause License
Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistribution's of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistribution's in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* The name of the copyright holders may not be used to endorse or promote products
derived from this software without specific prior written permission.
This software is provided by the copyright holders and contributors "as is" and
any express or implied warranties, including, but not limited to, the implied
warranties of merchantability and fitness for a particular purpose are disclaimed.
In no event shall the Intel Corporation or contributors be liable for any direct,
indirect, incidental, special, exemplary, or consequential damages
(including, but not limited to, procurement of substitute goods or services;
loss of use, data, or profits; or business interruption) however caused
and on any theory of liability, whether in contract, strict liability,
or tort (including negligence or otherwise) arising in any way out of
the use of this software, even if advised of the possibility of such damage.
*/
/**
* Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target
* Recognition on Mobile Devices: Revisiting Gaussian Elimination for the
* Estimation of Planar Homographies." In Computer Vision and Pattern
* Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125.
* IEEE, 2014.
*/
/* Include Guards */
#ifndef __OPENCV_RHO_H__
#define __OPENCV_RHO_H__
/* Includes */
#include <opencv2/core.hpp>
/* Defines */
/* Flags */
#ifndef RHO_FLAG_NONE
#define RHO_FLAG_NONE (0U<<0)
#endif
#ifndef RHO_FLAG_ENABLE_NR
#define RHO_FLAG_ENABLE_NR (1U<<0)
#endif
#ifndef RHO_FLAG_ENABLE_REFINEMENT
#define RHO_FLAG_ENABLE_REFINEMENT (1U<<1)
#endif
#ifndef RHO_FLAG_ENABLE_FINAL_REFINEMENT
#define RHO_FLAG_ENABLE_FINAL_REFINEMENT (1U<<2)
#endif
/* Namespace cv */
namespace cv{
/* Data structures */
/**
* Homography Estimation context.
*/
struct RHO_HEST;
typedef struct RHO_HEST RHO_HEST;
/* Functions */
/**
* Initialize the estimator context, by allocating the aligned buffers
* internally needed.
*
* @return A pointer to the context if successful; NULL if an error occurred.
*/
Ptr<RHO_HEST> rhoInit(void);
/**
* Ensure that the estimator context's internal table for non-randomness
* criterion is at least of the given size, and uses the given beta. The table
* should be larger than the maximum number of matches fed into the estimator.
*
* A value of N of 0 requests deallocation of the table.
*
* @param [in] p The initialized estimator context
* @param [in] N If 0, deallocate internal table. If > 0, ensure that the
* internal table is of at least this size, reallocating if
* necessary.
* @param [in] beta The beta-factor to use within the table.
* @return 0 if unsuccessful; non-zero otherwise.
*/
int rhoEnsureCapacity(Ptr<RHO_HEST> p, unsigned N, double beta);
/**
* Seeds the internal PRNG with the given seed.
*
* Although it is not required to call this function, since context
* initialization seeds itself with entropy from rand(), this function allows
* reproducible results by using a specified seed.
*
* @param [in] p The estimator context whose PRNG is to be seeded.
* @param [in] seed The 64-bit integer seed.
*/
void rhoSeed(Ptr<RHO_HEST> p, uint64_t seed);
/**
* Estimates the homography using the given context, matches and parameters to
* PROSAC.
*
* The given context must have been initialized.
*
* The matches are provided as two arrays of N single-precision, floating-point
* (x,y) points. Points with corresponding offsets in the two arrays constitute
* a match. The homography estimation attempts to find the 3x3 matrix H which
* best maps the homogeneous-coordinate points in the source array to their
* corresponding homogeneous-coordinate points in the destination array.
*
* Note: At least 4 matches must be provided (N >= 4).
* Note: A point in either array takes up 2 floats. The first of two stores
* the x-coordinate and the second of the two stores the y-coordinate.
* Thus, the arrays resemble this in memory:
*
* src = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...]
* Matches: | | | | |
* dst = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...]
* Note: The matches are expected to be provided sorted by quality, or at
* least not be worse-than-random in ordering.
*
* A pointer to the base of an array of N bytes can be provided. It serves as
* an output mask to indicate whether the corresponding match is an inlier to
* the returned homography, if any. A zero indicates an outlier; A non-zero
* value indicates an inlier.
*
* The PROSAC estimator requires a few parameters of its own. These are:
*
* - The maximum distance that a source point projected onto the destination
* plane can be from its putative match and still be considered an
* inlier. Must be non-negative.
* A sane default is 3.0.
* - The maximum number of PROSAC iterations. This corresponds to the
* largest number of samples that will be drawn and tested.
* A sane default is 2000.
* - The RANSAC convergence parameter. This corresponds to the number of
* iterations after which PROSAC will start sampling like RANSAC.
* A sane default is 2000.
* - The confidence threshold. This corresponds to the probability of
* finding a correct solution. Must be bounded by [0, 1].
* A sane default is 0.995.
* - The minimum number of inliers acceptable. Only a solution with at
* least this many inliers will be returned. The minimum is 4.
* A sane default is 10% of N.
* - The beta-parameter for the non-randomness termination criterion.
* Ignored if non-randomness criterion disabled, otherwise must be
* bounded by (0, 1).
* A sane default is 0.35.
* - Optional flags to control the estimation. Available flags are:
* HEST_FLAG_NONE:
* No special processing.
* HEST_FLAG_ENABLE_NR:
* Enable non-randomness criterion. If set, the beta parameter
* must also be set.
* HEST_FLAG_ENABLE_REFINEMENT:
* Enable refinement of each new best model, as they are found.
* HEST_FLAG_ENABLE_FINAL_REFINEMENT:
* Enable one final refinement of the best model found before
* returning it.
*
* The PROSAC estimator optionally accepts an extrinsic initial guess of H.
*
* The PROSAC estimator outputs a final estimate of H provided it was able to
* find one with a minimum of supporting inliers. If it was not, it outputs
* the all-zero matrix.
*
* The extrinsic guess at and final estimate of H are both in the same form:
* A 3x3 single-precision floating-point matrix with step 3. Thus, it is a
* 9-element array of floats, with the elements as follows:
*
* [ H00, H01, H02,
* H10, H11, H12,
* H20, H21, 1.0 ]
*
* Notice that the homography is normalized to H22 = 1.0.
*
* The function returns the number of inliers if it was able to find a
* homography with at least the minimum required support, and 0 if it was not.
*
*
* @param [in/out] p The context to use for homography estimation. Must
* be already initialized. Cannot be NULL.
* @param [in] src The pointer to the source points of the matches.
* Must be aligned to 4 bytes. Cannot be NULL.
* @param [in] dst The pointer to the destination points of the matches.
* Must be aligned to 4 bytes. Cannot be NULL.
* @param [out] inl The pointer to the output mask of inlier matches.
* Must be aligned to 4 bytes. May be NULL.
* @param [in] N The number of matches. Minimum 4.
* @param [in] maxD The maximum distance. Minimum 0.
* @param [in] maxI The maximum number of PROSAC iterations.
* @param [in] rConvg The RANSAC convergence parameter.
* @param [in] cfd The required confidence in the solution.
* @param [in] minInl The minimum required number of inliers. Minimum 4.
* @param [in] beta The beta-parameter for the non-randomness criterion.
* @param [in] flags A union of flags to fine-tune the estimation.
* @param [in] guessH An extrinsic guess at the solution H, or NULL if
* none provided.
* @param [out] finalH The final estimation of H, or the zero matrix if
* the minimum number of inliers was not met.
* Cannot be NULL.
* @return The number of inliers if the minimum number of
* inliers for acceptance was reached; 0 otherwise.
*/
unsigned rhoHest(Ptr<RHO_HEST> p, /* Homography estimation context. */
const float* src, /* Source points */
const float* dst, /* Destination points */
char* inl, /* Inlier mask */
unsigned N, /* = src.length = dst.length = inl.length */
float maxD, /* 3.0 */
unsigned maxI, /* 2000 */
unsigned rConvg, /* 2000 */
double cfd, /* 0.995 */
unsigned minInl, /* 4 */
double beta, /* 0.35 */
unsigned flags, /* 0 */
const float* guessH, /* Extrinsic guess, NULL if none provided */
float* finalH); /* Final result. */
/* End Namespace cv */
}
#endif

1056
Lib/opencv/sources/modules/calib3d/src/solvepnp.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1331
Lib/opencv/sources/modules/calib3d/src/stereobm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

2550
Lib/opencv/sources/modules/calib3d/src/stereosgbm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

388
Lib/opencv/sources/modules/calib3d/src/triangulate.cpp Normal file
View File

@@ -0,0 +1,388 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2009, Intel Corporation and others, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
// cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen.
// cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin.
// HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003.
// This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry
static void
icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
{
if( projMatr1 == 0 || projMatr2 == 0 ||
projPoints1 == 0 || projPoints2 == 0 ||
points4D == 0)
CV_Error( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) ||
!CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) ||
!CV_IS_MAT(points4D) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
int numPoints = projPoints1->cols;
if( numPoints < 1 )
CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" );
if( projPoints2->cols != numPoints || points4D->cols != numPoints )
CV_Error( CV_StsUnmatchedSizes, "Number of points must be the same" );
if( projPoints1->rows != 2 || projPoints2->rows != 2)
CV_Error( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
if( points4D->rows != 4 )
CV_Error( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
projMatr2->cols != 4 || projMatr2->rows != 3)
CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
// preallocate SVD matrices on stack
cv::Matx<double, 4, 4> matrA;
cv::Matx<double, 4, 4> matrU;
cv::Matx<double, 4, 1> matrW;
cv::Matx<double, 4, 4> matrV;
CvMat* projPoints[2] = {projPoints1, projPoints2};
CvMat* projMatrs[2] = {projMatr1, projMatr2};
/* Solve system for each point */
for( int i = 0; i < numPoints; i++ )/* For each point */
{
/* Fill matrix for current point */
for( int j = 0; j < 2; j++ )/* For each view */
{
double x,y;
x = cvmGet(projPoints[j],0,i);
y = cvmGet(projPoints[j],1,i);
for( int k = 0; k < 4; k++ )
{
matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k);
matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k);
}
}
/* Solve system for current point */
cv::SVD::compute(matrA, matrW, matrU, matrV);
/* Copy computed point */
cvmSet(points4D,0,i,matrV(3,0));/* X */
cvmSet(points4D,1,i,matrV(3,1));/* Y */
cvmSet(points4D,2,i,matrV(3,2));/* Z */
cvmSet(points4D,3,i,matrV(3,3));/* W */
}
}
/*
* The Optimal Triangulation Method (see HZ for details)
* For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F,
* computes the corrected correspondences new_points1[i] <-> new_points2[i] that minimize the
* geometric error d(points1[i],new_points1[i])^2 + d(points2[i],new_points2[i])^2 (where d(a,b)
* is the geometric distance between points a and b) subject to the epipolar constraint
* new_points2' * F * new_points1 = 0.
*
* F_ : 3x3 fundamental matrix
* points1_ : 1xN matrix containing the first set of points
* points2_ : 1xN matrix containing the second set of points
* new_points1 : the optimized points1_. if this is NULL, the corrected points are placed back in points1_
* new_points2 : the optimized points2_. if this is NULL, the corrected points are placed back in points2_
*/
static void
icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2)
{
cv::Ptr<CvMat> tmp33;
cv::Ptr<CvMat> tmp31, tmp31_2;
cv::Ptr<CvMat> T1i, T2i;
cv::Ptr<CvMat> R1, R2;
cv::Ptr<CvMat> TFT, TFTt, RTFTR;
cv::Ptr<CvMat> U, S, V;
cv::Ptr<CvMat> e1, e2;
cv::Ptr<CvMat> polynomial;
cv::Ptr<CvMat> result;
cv::Ptr<CvMat> points1, points2;
cv::Ptr<CvMat> F;
if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
if (!( F_->cols == 3 && F_->rows == 3))
CV_Error( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix");
if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 ))
CV_Error( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" );
if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols))
CV_Error( CV_StsUnmatchedSizes, "The point-matrices must have one row, and an equal number of columns" );
if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
CV_Error( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" );
if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
CV_Error( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" );
if (new_points1 != NULL) {
CV_Assert(CV_IS_MAT(new_points1));
if (new_points1->cols != points1_->cols || new_points1->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points1->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" );
}
if (new_points2 != NULL) {
CV_Assert(CV_IS_MAT(new_points2));
if (new_points2->cols != points2_->cols || new_points2->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points2->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" );
}
// Make sure F uses double precision
F.reset(cvCreateMat(3,3,CV_64FC1));
cvConvert(F_, F);
// Make sure points1 uses double precision
points1.reset(cvCreateMat(points1_->rows,points1_->cols,CV_64FC2));
cvConvert(points1_, points1);
// Make sure points2 uses double precision
points2.reset(cvCreateMat(points2_->rows,points2_->cols,CV_64FC2));
cvConvert(points2_, points2);
tmp33.reset(cvCreateMat(3,3,CV_64FC1));
tmp31.reset(cvCreateMat(3,1,CV_64FC1)), tmp31_2.reset(cvCreateMat(3,1,CV_64FC1));
T1i.reset(cvCreateMat(3,3,CV_64FC1)), T2i.reset(cvCreateMat(3,3,CV_64FC1));
R1.reset(cvCreateMat(3,3,CV_64FC1)), R2.reset(cvCreateMat(3,3,CV_64FC1));
TFT.reset(cvCreateMat(3,3,CV_64FC1)), TFTt.reset(cvCreateMat(3,3,CV_64FC1)), RTFTR.reset(cvCreateMat(3,3,CV_64FC1));
U.reset(cvCreateMat(3,3,CV_64FC1));
S.reset(cvCreateMat(3,3,CV_64FC1));
V.reset(cvCreateMat(3,3,CV_64FC1));
e1.reset(cvCreateMat(3,1,CV_64FC1)), e2.reset(cvCreateMat(3,1,CV_64FC1));
double x1, y1, x2, y2;
double scale;
double f1, f2, a, b, c, d;
polynomial.reset(cvCreateMat(1,7,CV_64FC1));
result.reset(cvCreateMat(1,6,CV_64FC2));
double t_min, s_val, t, s;
for (int p = 0; p < points1->cols; ++p) {
// Replace F by T2-t * F * T1-t
x1 = points1->data.db[p*2];
y1 = points1->data.db[p*2+1];
x2 = points2->data.db[p*2];
y2 = points2->data.db[p*2+1];
cvSetZero(T1i);
cvSetReal2D(T1i,0,0,1);
cvSetReal2D(T1i,1,1,1);
cvSetReal2D(T1i,2,2,1);
cvSetReal2D(T1i,0,2,x1);
cvSetReal2D(T1i,1,2,y1);
cvSetZero(T2i);
cvSetReal2D(T2i,0,0,1);
cvSetReal2D(T2i,1,1,1);
cvSetReal2D(T2i,2,2,1);
cvSetReal2D(T2i,0,2,x2);
cvSetReal2D(T2i,1,2,y2);
cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T);
cvSetZero(TFT);
cvGEMM(tmp33,T1i,1,0,0,TFT);
// Compute the right epipole e1 from F * e1 = 0
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFT,S,U,V);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e1,2,0) < 0) {
cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0));
cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0));
}
// Compute the left epipole e2 from e2' * F = 0 => F' * e2 = 0
cvSetZero(TFTt);
cvTranspose(TFT, TFTt);
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFTt,S,U,V);
cvSetZero(e2);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e2,2,0) < 0) {
cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0));
cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0));
}
// Replace F by R2 * F * R1'
cvSetZero(R1);
cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,2,2,1);
cvSetZero(R2);
cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,2,2,1);
cvGEMM(R2,TFT,1,0,0,tmp33);
cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T);
// Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33
f1 = cvGetReal2D(e1,2,0);
f2 = cvGetReal2D(e2,2,0);
a = cvGetReal2D(RTFTR,1,1);
b = cvGetReal2D(RTFTR,1,2);
c = cvGetReal2D(RTFTR,2,1);
d = cvGetReal2D(RTFTR,2,2);
// Form the polynomial g(t) = k6*t^6 + k5*t^5 + k4*t^4 + k3*t^3 + k2*t^2 + k1*t + k0
// from f1, f2, a, b, c and d
cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a ));
cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d ));
cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d ));
cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d ));
cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c ));
cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d ));
// Solve g(t) for t to get 6 roots
cvSetZero(result);
cvSolvePoly(polynomial, result, 100, 20);
// Evaluate the cost function s(t) at the real part of the 6 roots
t_min = DBL_MAX;
s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
for (int ti = 0; ti < 6; ++ti) {
t = result->data.db[2*ti];
s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d));
if (s < s_val) {
s_val = s;
t_min = t;
}
}
// find the optimal x1 and y1 as the points on l1 and l2 closest to the origin
tmp31->data.db[0] = t_min*t_min*f1;
tmp31->data.db[1] = t_min;
tmp31->data.db[2] = t_min*t_min*f1*f1+1;
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x1 = tmp31_2->data.db[0];
y1 = tmp31_2->data.db[1];
tmp31->data.db[0] = f2*pow(c*t_min+d,2);
tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d);
tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2);
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x2 = tmp31_2->data.db[0];
y2 = tmp31_2->data.db[1];
// Return the points in the matrix format that the user wants
points1->data.db[p*2] = x1;
points1->data.db[p*2+1] = y1;
points2->data.db[p*2] = x2;
points2->data.db[p*2+1] = y2;
}
if( new_points1 )
cvConvert( points1, new_points1 );
if( new_points2 )
cvConvert( points2, new_points2 );
}
void cv::triangulatePoints( InputArray _projMatr1, InputArray _projMatr2,
InputArray _projPoints1, InputArray _projPoints2,
OutputArray _points4D )
{
CV_INSTRUMENT_REGION();
Mat matr1 = _projMatr1.getMat(), matr2 = _projMatr2.getMat();
Mat points1 = _projPoints1.getMat(), points2 = _projPoints2.getMat();
if((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2)
points1 = points1.reshape(1, static_cast<int>(points1.total())).t();
if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2)
points2 = points2.reshape(1, static_cast<int>(points2.total())).t();
CvMat cvMatr1 = cvMat(matr1), cvMatr2 = cvMat(matr2);
CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);
_points4D.create(4, points1.cols, points1.type());
Mat cvPoints4D_ = _points4D.getMat();
CvMat cvPoints4D = cvMat(cvPoints4D_);
icvTriangulatePoints(&cvMatr1, &cvMatr2, &cvPoints1, &cvPoints2, &cvPoints4D);
}
void cv::correctMatches( InputArray _F, InputArray _points1, InputArray _points2,
OutputArray _newPoints1, OutputArray _newPoints2 )
{
CV_INSTRUMENT_REGION();
Mat F = _F.getMat();
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2);
CvMat cvF = cvMat(F);
_newPoints1.create(points1.size(), points1.type());
_newPoints2.create(points2.size(), points2.type());
Mat cvNewPoints1_ = _newPoints1.getMat(), cvNewPoints2_ = _newPoints2.getMat();
CvMat cvNewPoints1 = cvMat(cvNewPoints1_), cvNewPoints2 = cvMat(cvNewPoints2_);
icvCorrectMatches(&cvF, &cvPoints1, &cvPoints2, &cvNewPoints1, &cvNewPoints2);
}

645
Lib/opencv/sources/modules/calib3d/src/undistort.dispatch.cpp Normal file
View File

@@ -0,0 +1,645 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "distortion_model.hpp"
#include "calib3d_c_api.h"
#include "undistort.simd.hpp"
#include "undistort.simd_declarations.hpp" // defines CV_CPU_DISPATCH_MODES_ALL=AVX2,...,BASELINE based on CMakeLists.txt content
namespace cv
{
Mat getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize,
bool centerPrincipalPoint )
{
Mat cameraMatrix = _cameraMatrix.getMat();
if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
return cameraMatrix;
Mat newCameraMatrix;
cameraMatrix.convertTo(newCameraMatrix, CV_64F);
if( centerPrincipalPoint )
{
newCameraMatrix.ptr<double>()[2] = (imgsize.width-1)*0.5;
newCameraMatrix.ptr<double>()[5] = (imgsize.height-1)*0.5;
}
return newCameraMatrix;
}
namespace {
Ptr<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type,
const double* _ir, Matx33d &_matTilt,
double _u0, double _v0, double _fx, double _fy,
double _k1, double _k2, double _p1, double _p2,
double _k3, double _k4, double _k5, double _k6,
double _s1, double _s2, double _s3, double _s4)
{
CV_INSTRUMENT_REGION();
CV_CPU_DISPATCH(getInitUndistortRectifyMapComputer, (_size, _map1, _map2, _m1type, _ir, _matTilt, _u0, _v0, _fx, _fy, _k1, _k2, _p1, _p2, _k3, _k4, _k5, _k6, _s1, _s2, _s3, _s4),
CV_CPU_DISPATCH_MODES_ALL);
}
}
void initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs,
InputArray _matR, InputArray _newCameraMatrix,
Size size, int m1type, OutputArray _map1, OutputArray _map2 )
{
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat();
if( m1type <= 0 )
m1type = CV_16SC2;
CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
_map1.create( size, m1type );
Mat map1 = _map1.getMat(), map2;
if( m1type != CV_32FC2 )
{
_map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
map2 = _map2.getMat();
}
else
_map2.release();
Mat_<double> R = Mat_<double>::eye(3, 3);
Mat_<double> A = Mat_<double>(cameraMatrix), Ar;
if( !newCameraMatrix.empty() )
Ar = Mat_<double>(newCameraMatrix);
else
Ar = getDefaultNewCameraMatrix( A, size, true );
if( !matR.empty() )
R = Mat_<double>(matR);
if( !distCoeffs.empty() )
distCoeffs = Mat_<double>(distCoeffs);
else
{
distCoeffs.create(14, 1, CV_64F);
distCoeffs = 0.;
}
CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );
CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));
Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
const double* ir = &iR(0,0);
double u0 = A(0, 2), v0 = A(1, 2);
double fx = A(0, 0), fy = A(1, 1);
CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) ||
distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) ||
distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1) ||
distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1) ||
distCoeffs.size() == Size(1, 14) || distCoeffs.size() == Size(14, 1));
if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
distCoeffs = distCoeffs.t();
const double* const distPtr = distCoeffs.ptr<double>();
double k1 = distPtr[0];
double k2 = distPtr[1];
double p1 = distPtr[2];
double p2 = distPtr[3];
double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? distPtr[4] : 0.;
double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[5] : 0.;
double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[6] : 0.;
double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[7] : 0.;
double s1 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[8] : 0.;
double s2 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[9] : 0.;
double s3 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[10] : 0.;
double s4 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[11] : 0.;
double tauX = distCoeffs.cols + distCoeffs.rows - 1 >= 14 ? distPtr[12] : 0.;
double tauY = distCoeffs.cols + distCoeffs.rows - 1 >= 14 ? distPtr[13] : 0.;
// Matrix for trapezoidal distortion of tilted image sensor
Matx33d matTilt = Matx33d::eye();
detail::computeTiltProjectionMatrix(tauX, tauY, &matTilt);
parallel_for_(Range(0, size.height), *getInitUndistortRectifyMapComputer(
size, map1, map2, m1type, ir, matTilt, u0, v0,
fx, fy, k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4));
}
void undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix,
InputArray _distCoeffs, InputArray _newCameraMatrix )
{
CV_INSTRUMENT_REGION();
Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
Mat distCoeffs = _distCoeffs.getMat(), newCameraMatrix = _newCameraMatrix.getMat();
_dst.create( src.size(), src.type() );
Mat dst = _dst.getMat();
CV_Assert( dst.data != src.data );
int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);
Mat_<double> A, Ar, I = Mat_<double>::eye(3,3);
cameraMatrix.convertTo(A, CV_64F);
if( !distCoeffs.empty() )
distCoeffs = Mat_<double>(distCoeffs);
else
{
distCoeffs.create(5, 1, CV_64F);
distCoeffs = 0.;
}
if( !newCameraMatrix.empty() )
newCameraMatrix.convertTo(Ar, CV_64F);
else
A.copyTo(Ar);
double v0 = Ar(1, 2);
for( int y = 0; y < src.rows; y += stripe_size0 )
{
int stripe_size = std::min( stripe_size0, src.rows - y );
Ar(1, 2) = v0 - y;
Mat map1_part = map1.rowRange(0, stripe_size),
map2_part = map2.rowRange(0, stripe_size),
dst_part = dst.rowRange(y, y + stripe_size);
initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
map1_part.type(), map1_part, map2_part );
remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_CONSTANT );
}
}
}
CV_IMPL void
cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr )
{
cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA;
if( newAarr )
newA = cv::cvarrToMat(newAarr);
CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
cv::undistort( src, dst, A, distCoeffs, newA );
}
CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
CvArr* mapxarr, CvArr* mapyarr )
{
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
if( mapyarr )
mapy0 = mapy = cv::cvarrToMat(mapyarr);
cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
mapx.size(), mapx.type(), mapx, mapy );
CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}
void
cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
{
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
if( mapyarr )
mapy0 = mapy = cv::cvarrToMat(mapyarr);
if( dist_coeffs )
distCoeffs = cv::cvarrToMat(dist_coeffs);
if( Rarr )
R = cv::cvarrToMat(Rarr);
if( ArArr )
Ar = cv::cvarrToMat(ArArr);
cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}
static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria)
{
CV_Assert(criteria.isValid());
double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR);
cv::Matx33d invMatTilt = cv::Matx33d::eye();
cv::Matx33d matTilt = cv::Matx33d::eye();
CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
_src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_Assert( CV_IS_MAT(_cameraMatrix) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
cvConvert( _cameraMatrix, &matA );
if( _distCoeffs )
{
CV_Assert( CV_IS_MAT(_distCoeffs) &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5 ||
_distCoeffs->rows*_distCoeffs->cols == 8 ||
_distCoeffs->rows*_distCoeffs->cols == 12 ||
_distCoeffs->rows*_distCoeffs->cols == 14));
_Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
cvConvert( _distCoeffs, &_Dk );
if (k[12] != 0 || k[13] != 0)
{
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
}
}
if( matR )
{
CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
cvConvert( matR, &_RR );
}
else
cvSetIdentity(&_RR);
if( matP )
{
double PP[3][3];
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
int stype = CV_MAT_TYPE(_src->type);
int dtype = CV_MAT_TYPE(_dst->type);
int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
double fx = A[0][0];
double fy = A[1][1];
double ifx = 1./fx;
double ify = 1./fy;
double cx = A[0][2];
double cy = A[1][2];
int n = _src->rows + _src->cols - 1;
for( int i = 0; i < n; i++ )
{
double x, y, x0 = 0, y0 = 0, u, v;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
u = x; v = y;
x = (x - cx)*ifx;
y = (y - cy)*ify;
if( _distCoeffs ) {
// compensate tilt distortion
cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
x0 = x = invProj * vecUntilt(0);
y0 = y = invProj * vecUntilt(1);
double error = std::numeric_limits<double>::max();
// compensate distortion iteratively
for( int j = 0; ; j++ )
{
if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
break;
if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
break;
double r2 = x*x + y*y;
double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
if (icdist < 0) // test: undistortPoints.regression_14583
{
x = (u - cx)*ifx;
y = (v - cy)*ify;
break;
}
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
if(criteria.type & cv::TermCriteria::EPS)
{
double r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd, xd0, yd0;
cv::Vec3d vecTilt;
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
xd = invProj * vecTilt(0);
yd = invProj * vecTilt(1);
double x_proj = xd*fx + cx;
double y_proj = yd*fy + cy;
error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
}
}
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
}
void cvUndistortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP)
{
cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP,
cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01));
}
namespace cv {
void undistortPoints(InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat,
InputArray _Pmat)
{
undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, _Rmat, _Pmat, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
}
void undistortPoints(InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat,
InputArray _Pmat,
TermCriteria criteria)
{
Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();
int npoints = src.checkVector(2), depth = src.depth();
if (npoints < 0)
src = src.t();
npoints = src.checkVector(2);
CV_Assert(npoints >= 0 && src.isContinuous() && (depth == CV_32F || depth == CV_64F));
if (src.cols == 2)
src = src.reshape(2);
_dst.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
Mat dst = _dst.getMat();
CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _ccameraMatrix = cvMat(cameraMatrix);
CvMat matR, matP, _cdistCoeffs, *pR=0, *pP=0, *pD=0;
if( !R.empty() )
pR = &(matR = cvMat(R));
if( !P.empty() )
pP = &(matP = cvMat(P));
if( !distCoeffs.empty() )
pD = &(_cdistCoeffs = cvMat(distCoeffs));
cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria);
}
static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, enum UndistortTypes projType)
{
double x = p.x, y = p.y;
double beta = 1 + 2*alpha;
double v = x*x + y*y + 1, iv = 1/v;
double u = std::sqrt(beta*v + alpha*alpha);
double k = (u - alpha)*iv;
double kv = (v*beta/u - (u - alpha)*2)*iv*iv;
double kx = kv*x, ky = kv*y;
if( projType == PROJ_SPHERICAL_ORTHO )
{
if(J)
*J = Vec4d(kx*x + k, kx*y, ky*x, ky*y + k);
return Point2f((float)(x*k), (float)(y*k));
}
if( projType == PROJ_SPHERICAL_EQRECT )
{
// equirectangular
double iR = 1/(alpha + 1);
double x1 = std::max(std::min(x*k*iR, 1.), -1.);
double y1 = std::max(std::min(y*k*iR, 1.), -1.);
if(J)
{
double fx1 = iR/std::sqrt(1 - x1*x1);
double fy1 = iR/std::sqrt(1 - y1*y1);
*J = Vec4d(fx1*(kx*x + k), fx1*ky*x, fy1*kx*y, fy1*(ky*y + k));
}
return Point2f((float)asin(x1), (float)asin(y1));
}
CV_Error(Error::StsBadArg, "Unknown projection type");
}
static Point2f invMapPointSpherical(Point2f _p, float alpha, enum UndistortTypes projType)
{
double eps = 1e-12;
Vec2d p(_p.x, _p.y), q(_p.x, _p.y), err;
Vec4d J;
int i, maxiter = 5;
for( i = 0; i < maxiter; i++ )
{
Point2f p1 = mapPointSpherical(Point2f((float)q[0], (float)q[1]), alpha, &J, projType);
err = Vec2d(p1.x, p1.y) - p;
if( err[0]*err[0] + err[1]*err[1] < eps )
break;
Vec4d JtJ(J[0]*J[0] + J[2]*J[2], J[0]*J[1] + J[2]*J[3],
J[0]*J[1] + J[2]*J[3], J[1]*J[1] + J[3]*J[3]);
double d = JtJ[0]*JtJ[3] - JtJ[1]*JtJ[2];
d = d ? 1./d : 0;
Vec4d iJtJ(JtJ[3]*d, -JtJ[1]*d, -JtJ[2]*d, JtJ[0]*d);
Vec2d JtErr(J[0]*err[0] + J[2]*err[1], J[1]*err[0] + J[3]*err[1]);
q -= Vec2d(iJtJ[0]*JtErr[0] + iJtJ[1]*JtErr[1], iJtJ[2]*JtErr[0] + iJtJ[3]*JtErr[1]);
//Matx22d J(kx*x + k, kx*y, ky*x, ky*y + k);
//q -= Vec2d((J.t()*J).inv()*(J.t()*err));
}
return i < maxiter ? Point2f((float)q[0], (float)q[1]) : Point2f(-FLT_MAX, -FLT_MAX);
}
float initWideAngleProjMap(InputArray _cameraMatrix0, InputArray _distCoeffs0,
Size imageSize, int destImageWidth, int m1type,
OutputArray _map1, OutputArray _map2,
enum UndistortTypes projType, double _alpha)
{
Mat cameraMatrix0 = _cameraMatrix0.getMat(), distCoeffs0 = _distCoeffs0.getMat();
double k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0};
Mat distCoeffs(distCoeffs0.rows, distCoeffs0.cols, CV_MAKETYPE(CV_64F,distCoeffs0.channels()), k);
Mat cameraMatrix(3,3,CV_64F,M);
Point2f scenter((float)cameraMatrix.at<double>(0,2), (float)cameraMatrix.at<double>(1,2));
Point2f dcenter((destImageWidth-1)*0.5f, 0.f);
float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX;
int N = 9;
std::vector<Point2f> uvec(1), vvec(1);
Mat I = Mat::eye(3,3,CV_64F);
float alpha = (float)_alpha;
int ndcoeffs = distCoeffs0.cols*distCoeffs0.rows*distCoeffs0.channels();
CV_Assert((distCoeffs0.cols == 1 || distCoeffs0.rows == 1) &&
(ndcoeffs == 4 || ndcoeffs == 5 || ndcoeffs == 8 || ndcoeffs == 12 || ndcoeffs == 14));
CV_Assert(cameraMatrix0.size() == Size(3,3));
distCoeffs0.convertTo(distCoeffs,CV_64F);
cameraMatrix0.convertTo(cameraMatrix,CV_64F);
alpha = std::min(alpha, 0.999f);
for( int i = 0; i < N; i++ )
for( int j = 0; j < N; j++ )
{
Point2f p((float)j*imageSize.width/(N-1), (float)i*imageSize.height/(N-1));
uvec[0] = p;
undistortPoints(uvec, vvec, cameraMatrix, distCoeffs, I, I);
Point2f q = mapPointSpherical(vvec[0], alpha, 0, projType);
if( xmin > q.x ) xmin = q.x;
if( xmax < q.x ) xmax = q.x;
if( ymin > q.y ) ymin = q.y;
if( ymax < q.y ) ymax = q.y;
}
float scale = (float)std::min(dcenter.x/fabs(xmax), dcenter.x/fabs(xmin));
Size dsize(destImageWidth, cvCeil(std::max(scale*fabs(ymin)*2, scale*fabs(ymax)*2)));
dcenter.y = (dsize.height - 1)*0.5f;
Mat mapxy(dsize, CV_32FC2);
double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7], s1 = k[8], s2 = k[9], s3 = k[10], s4 = k[11];
double fx = cameraMatrix.at<double>(0,0), fy = cameraMatrix.at<double>(1,1), cx = scenter.x, cy = scenter.y;
cv::Matx33d matTilt;
cv::detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt);
for( int y = 0; y < dsize.height; y++ )
{
Point2f* mxy = mapxy.ptr<Point2f>(y);
for( int x = 0; x < dsize.width; x++ )
{
Point2f p = (Point2f((float)x, (float)y) - dcenter)*(1.f/scale);
Point2f q = invMapPointSpherical(p, alpha, projType);
if( q.x <= -FLT_MAX && q.y <= -FLT_MAX )
{
mxy[x] = Point2f(-1.f, -1.f);
continue;
}
double x2 = q.x*q.x, y2 = q.y*q.y;
double r2 = x2 + y2, _2xy = 2*q.x*q.y;
double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
double xd = (q.x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+ s2*r2*r2);
double yd = (q.y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+ s4*r2*r2);
cv::Vec3d vecTilt = matTilt*cv::Vec3d(xd, yd, 1);
double invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
double u = fx*invProj*vecTilt(0) + cx;
double v = fy*invProj*vecTilt(1) + cy;
mxy[x] = Point2f((float)u, (float)v);
}
}
if(m1type == CV_32FC2)
{
_map1.create(mapxy.size(), mapxy.type());
Mat map1 = _map1.getMat();
mapxy.copyTo(map1);
_map2.release();
}
else
convertMaps(mapxy, Mat(), _map1, _map2, m1type, false);
return scale;
}
} // namespace
/* End of file */

329
Lib/opencv/sources/modules/calib3d/src/undistort.simd.hpp Normal file
View File

@@ -0,0 +1,329 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "opencv2/core/hal/intrin.hpp"
namespace cv {
CV_CPU_OPTIMIZATION_NAMESPACE_BEGIN
// forward declarations
Ptr<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type,
const double* _ir, Matx33d &_matTilt,
double _u0, double _v0, double _fx, double _fy,
double _k1, double _k2, double _p1, double _p2,
double _k3, double _k4, double _k5, double _k6,
double _s1, double _s2, double _s3, double _s4);
#ifndef CV_CPU_OPTIMIZATION_DECLARATIONS_ONLY
namespace
{
class initUndistortRectifyMapComputer : public ParallelLoopBody
{
public:
initUndistortRectifyMapComputer(
Size _size, Mat &_map1, Mat &_map2, int _m1type,
const double* _ir, Matx33d &_matTilt,
double _u0, double _v0, double _fx, double _fy,
double _k1, double _k2, double _p1, double _p2,
double _k3, double _k4, double _k5, double _k6,
double _s1, double _s2, double _s3, double _s4)
: size(_size),
map1(_map1),
map2(_map2),
m1type(_m1type),
ir(_ir),
matTilt(_matTilt),
u0(_u0),
v0(_v0),
fx(_fx),
fy(_fy),
k1(_k1),
k2(_k2),
p1(_p1),
p2(_p2),
k3(_k3),
k4(_k4),
k5(_k5),
k6(_k6),
s1(_s1),
s2(_s2),
s3(_s3),
s4(_s4) {
#if CV_SIMD_64F
for (int i = 0; i < 2 * v_float64::nlanes; ++i)
{
s_x[i] = ir[0] * i;
s_y[i] = ir[3] * i;
s_w[i] = ir[6] * i;
}
#endif
}
void operator()( const cv::Range& range ) const CV_OVERRIDE
{
CV_INSTRUMENT_REGION();
const int begin = range.start;
const int end = range.end;
for( int i = begin; i < end; i++ )
{
float* m1f = map1.ptr<float>(i);
float* m2f = map2.empty() ? 0 : map2.ptr<float>(i);
short* m1 = (short*)m1f;
ushort* m2 = (ushort*)m2f;
double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
int j = 0;
if (m1type == CV_16SC2)
CV_Assert(m1 != NULL && m2 != NULL);
else if (m1type == CV_32FC1)
CV_Assert(m1f != NULL && m2f != NULL);
else
CV_Assert(m1 != NULL);
#if CV_SIMD_64F
const v_float64 v_one = vx_setall_f64(1.0);
for (; j <= size.width - 2*v_float64::nlanes; j += 2*v_float64::nlanes, _x += 2*v_float64::nlanes * ir[0], _y += 2*v_float64::nlanes * ir[3], _w += 2*v_float64::nlanes * ir[6])
{
v_float64 m_0, m_1, m_2, m_3;
m_2 = v_one / (vx_setall_f64(_w) + vx_load(s_w));
m_3 = v_one / (vx_setall_f64(_w) + vx_load(s_w + v_float64::nlanes));
m_0 = vx_setall_f64(_x); m_1 = vx_setall_f64(_y);
v_float64 x_0 = (m_0 + vx_load(s_x)) * m_2;
v_float64 x_1 = (m_0 + vx_load(s_x + v_float64::nlanes)) * m_3;
v_float64 y_0 = (m_1 + vx_load(s_y)) * m_2;
v_float64 y_1 = (m_1 + vx_load(s_y + v_float64::nlanes)) * m_3;
v_float64 xd_0 = x_0 * x_0;
v_float64 yd_0 = y_0 * y_0;
v_float64 xd_1 = x_1 * x_1;
v_float64 yd_1 = y_1 * y_1;
v_float64 r2_0 = xd_0 + yd_0;
v_float64 r2_1 = xd_1 + yd_1;
m_1 = vx_setall_f64(k3);
m_2 = vx_setall_f64(k2);
m_3 = vx_setall_f64(k1);
m_0 = v_muladd(v_muladd(v_muladd(m_1, r2_0, m_2), r2_0, m_3), r2_0, v_one);
m_1 = v_muladd(v_muladd(v_muladd(m_1, r2_1, m_2), r2_1, m_3), r2_1, v_one);
m_3 = vx_setall_f64(k6);
m_2 = vx_setall_f64(k5);
m_0 /= v_muladd(v_muladd(v_muladd(m_3, r2_0, m_2), r2_0, vx_setall_f64(k4)), r2_0, v_one);
m_1 /= v_muladd(v_muladd(v_muladd(m_3, r2_1, m_2), r2_1, vx_setall_f64(k4)), r2_1, v_one);
m_3 = vx_setall_f64(2.0);
xd_0 = v_muladd(m_3, xd_0, r2_0);
yd_0 = v_muladd(m_3, yd_0, r2_0);
xd_1 = v_muladd(m_3, xd_1, r2_1);
yd_1 = v_muladd(m_3, yd_1, r2_1);
m_2 = x_0 * y_0 * m_3;
m_3 = x_1 * y_1 * m_3;
x_0 *= m_0; y_0 *= m_0; x_1 *= m_1; y_1 *= m_1;
m_0 = vx_setall_f64(p1);
m_1 = vx_setall_f64(p2);
xd_0 = v_muladd(xd_0, m_1, x_0);
yd_0 = v_muladd(yd_0, m_0, y_0);
xd_1 = v_muladd(xd_1, m_1, x_1);
yd_1 = v_muladd(yd_1, m_0, y_1);
xd_0 = v_muladd(m_0, m_2, xd_0);
yd_0 = v_muladd(m_1, m_2, yd_0);
xd_1 = v_muladd(m_0, m_3, xd_1);
yd_1 = v_muladd(m_1, m_3, yd_1);
m_0 = r2_0 * r2_0;
m_1 = r2_1 * r2_1;
m_2 = vx_setall_f64(s2);
m_3 = vx_setall_f64(s1);
xd_0 = v_muladd(m_3, r2_0, v_muladd(m_2, m_0, xd_0));
xd_1 = v_muladd(m_3, r2_1, v_muladd(m_2, m_1, xd_1));
m_2 = vx_setall_f64(s4);
m_3 = vx_setall_f64(s3);
yd_0 = v_muladd(m_3, r2_0, v_muladd(m_2, m_0, yd_0));
yd_1 = v_muladd(m_3, r2_1, v_muladd(m_2, m_1, yd_1));
m_0 = vx_setall_f64(matTilt.val[0]);
m_1 = vx_setall_f64(matTilt.val[1]);
m_2 = vx_setall_f64(matTilt.val[2]);
x_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2));
x_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2));
m_0 = vx_setall_f64(matTilt.val[3]);
m_1 = vx_setall_f64(matTilt.val[4]);
m_2 = vx_setall_f64(matTilt.val[5]);
y_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2));
y_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2));
m_0 = vx_setall_f64(matTilt.val[6]);
m_1 = vx_setall_f64(matTilt.val[7]);
m_2 = vx_setall_f64(matTilt.val[8]);
r2_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2));
r2_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2));
m_0 = vx_setzero_f64();
r2_0 = v_select(r2_0 == m_0, v_one, v_one / r2_0);
r2_1 = v_select(r2_1 == m_0, v_one, v_one / r2_1);
m_0 = vx_setall_f64(fx);
m_1 = vx_setall_f64(u0);
m_2 = vx_setall_f64(fy);
m_3 = vx_setall_f64(v0);
x_0 = v_muladd(m_0 * r2_0, x_0, m_1);
y_0 = v_muladd(m_2 * r2_0, y_0, m_3);
x_1 = v_muladd(m_0 * r2_1, x_1, m_1);
y_1 = v_muladd(m_2 * r2_1, y_1, m_3);
if (m1type == CV_32FC1)
{
v_store(&m1f[j], v_cvt_f32(x_0, x_1));
v_store(&m2f[j], v_cvt_f32(y_0, y_1));
}
else if (m1type == CV_32FC2)
{
v_float32 mf0, mf1;
v_zip(v_cvt_f32(x_0, x_1), v_cvt_f32(y_0, y_1), mf0, mf1);
v_store(&m1f[j * 2], mf0);
v_store(&m1f[j * 2 + v_float32::nlanes], mf1);
}
else // m1type == CV_16SC2
{
m_0 = vx_setall_f64(INTER_TAB_SIZE);
x_0 *= m_0; x_1 *= m_0; y_0 *= m_0; y_1 *= m_0;
v_int32 mask = vx_setall_s32(INTER_TAB_SIZE - 1);
v_int32 iu = v_round(x_0, x_1);
v_int32 iv = v_round(y_0, y_1);
v_pack_u_store(&m2[j], (iu & mask) + (iv & mask) * vx_setall_s32(INTER_TAB_SIZE));
v_int32 out0, out1;
v_zip(iu >> INTER_BITS, iv >> INTER_BITS, out0, out1);
v_store(&m1[j * 2], v_pack(out0, out1));
}
}
vx_cleanup();
#endif
for( ; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
{
double w = 1./_w, x = _x*w, y = _y*w;
double x2 = x*x, y2 = y*y;
double r2 = x2 + y2, _2xy = 2*x*y;
double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
double xd = (x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2);
double yd = (y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2);
Vec3d vecTilt = matTilt*cv::Vec3d(xd, yd, 1);
double invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
double u = fx*invProj*vecTilt(0) + u0;
double v = fy*invProj*vecTilt(1) + v0;
if( m1type == CV_16SC2 )
{
int iu = saturate_cast<int>(u*INTER_TAB_SIZE);
int iv = saturate_cast<int>(v*INTER_TAB_SIZE);
m1[j*2] = (short)(iu >> INTER_BITS);
m1[j*2+1] = (short)(iv >> INTER_BITS);
m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));
}
else if( m1type == CV_32FC1 )
{
m1f[j] = (float)u;
m2f[j] = (float)v;
}
else
{
m1f[j*2] = (float)u;
m1f[j*2+1] = (float)v;
}
}
}
}
private:
Size size;
Mat &map1;
Mat &map2;
int m1type;
const double* ir;
Matx33d &matTilt;
double u0;
double v0;
double fx;
double fy;
double k1;
double k2;
double p1;
double p2;
double k3;
double k4;
double k5;
double k6;
double s1;
double s2;
double s3;
double s4;
#if CV_SIMD_64F
double s_x[2*v_float64::nlanes];
double s_y[2*v_float64::nlanes];
double s_w[2*v_float64::nlanes];
#endif
};
}
Ptr<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type,
const double* _ir, Matx33d &_matTilt,
double _u0, double _v0, double _fx, double _fy,
double _k1, double _k2, double _p1, double _p2,
double _k3, double _k4, double _k5, double _k6,
double _s1, double _s2, double _s3, double _s4)
{
CV_INSTRUMENT_REGION();
return Ptr<initUndistortRectifyMapComputer>(new initUndistortRectifyMapComputer(_size, _map1, _map2, _m1type, _ir, _matTilt, _u0, _v0, _fx, _fy,
_k1, _k2, _p1, _p2, _k3, _k4, _k5, _k6, _s1, _s2, _s3, _s4));
}
#endif
CV_CPU_OPTIMIZATION_NAMESPACE_END
}
/* End of file */

829
Lib/opencv/sources/modules/calib3d/src/upnp.cpp Normal file
View File

@@ -0,0 +1,829 @@
//M*//////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
/****************************************************************************************\
* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
* Contributed by Edgar Riba
\****************************************************************************************/
#include "precomp.hpp"
#include "upnp.h"
#include <limits>
#if 0 // fix buffer overflow first (FIXIT mark in .cpp file)
using namespace std;
using namespace cv;
upnp::upnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
pws.resize(3 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<Point3f,Point2f>(opoints, ipoints);
else
init_points<Point3d,Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<Point3f,Point2d>(opoints, ipoints);
else
init_points<Point3d,Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
max_nr = 0;
A1 = NULL;
A2 = NULL;
}
upnp::~upnp()
{
if (A1)
delete[] A1;
if (A2)
delete[] A2;
}
double upnp::compute_pose(Mat& R, Mat& t)
{
choose_control_points();
compute_alphas();
Mat * M = new Mat(2 * number_of_correspondences, 12, CV_64F);
for(int i = 0; i < number_of_correspondences; i++)
{
fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);
}
double mtm[12 * 12], d[12], ut[12 * 12], vt[12 * 12];
Mat MtM = Mat(12, 12, CV_64F, mtm);
Mat D = Mat(12, 1, CV_64F, d);
Mat Ut = Mat(12, 12, CV_64F, ut);
Mat Vt = Mat(12, 12, CV_64F, vt);
MtM = M->t() * (*M);
SVD::compute(MtM, D, Ut, Vt, SVD::MODIFY_A | SVD::FULL_UV);
Mat(Ut.t()).copyTo(Ut);
M->release();
delete M;
double l_6x12[6 * 12], rho[6];
Mat L_6x12 = Mat(6, 12, CV_64F, l_6x12);
Mat Rho = Mat(6, 1, CV_64F, rho);
compute_L_6x12(ut, l_6x12);
compute_rho(rho);
double Betas[3][4], Efs[3][1], rep_errors[3];
double Rs[3][3][3], ts[3][3];
find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]);
gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]);
rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]);
gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
int N = 1;
if (rep_errors[2] < rep_errors[1]) N = 2;
Mat(3, 1, CV_64F, ts[N]).copyTo(t);
Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
fu = fv = Efs[N][0];
return fu;
}
void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
double R_dst[3][3], double t_dst[3])
{
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R_dst[i][j] = R_src[i][j];
t_dst[i] = t_src[i];
}
}
void upnp::estimate_R_and_t(double R[3][3], double t[3])
{
double pc0[3], pw0[3];
pc0[0] = pc0[1] = pc0[2] = 0.0;
pw0[0] = pw0[1] = pw0[2] = 0.0;
for(int i = 0; i < number_of_correspondences; i++) {
const double * pc = &pcs[3 * i];
const double * pw = &pws[3 * i];
for(int j = 0; j < 3; j++) {
pc0[j] += pc[j];
pw0[j] += pw[j];
}
}
for(int j = 0; j < 3; j++) {
pc0[j] /= number_of_correspondences;
pw0[j] /= number_of_correspondences;
}
double abt[3 * 3] = {0}, abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
Mat ABt = Mat(3, 3, CV_64F, abt);
Mat ABt_D = Mat(3, 1, CV_64F, abt_d);
Mat ABt_U = Mat(3, 3, CV_64F, abt_u);
Mat ABt_V = Mat(3, 3, CV_64F, abt_v);
ABt.setTo(0.0);
for(int i = 0; i < number_of_correspondences; i++) {
double * pc = &pcs[3 * i];
double * pw = &pws[3 * i];
for(int j = 0; j < 3; j++) {
abt[3 * j ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
}
}
SVD::compute(ABt, ABt_D, ABt_U, ABt_V, SVD::MODIFY_A);
Mat(ABt_V.t()).copyTo(ABt_V);
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
const double det =
R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
if (det < 0) {
R[2][0] = -R[2][0];
R[2][1] = -R[2][1];
R[2][2] = -R[2][2];
}
t[0] = pc0[0] - dot(R[0], pw0);
t[1] = pc0[1] - dot(R[1], pw0);
t[2] = pc0[2] - dot(R[2], pw0);
}
void upnp::solve_for_sign(void)
{
if (pcs[2] < 0.0) {
for(int i = 0; i < 4; i++)
for(int j = 0; j < 3; j++)
ccs[i][j] = -ccs[i][j];
for(int i = 0; i < number_of_correspondences; i++) {
pcs[3 * i ] = -pcs[3 * i];
pcs[3 * i + 1] = -pcs[3 * i + 1];
pcs[3 * i + 2] = -pcs[3 * i + 2];
}
}
}
double upnp::compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3])
{
compute_ccs(betas, ut);
compute_pcs();
solve_for_sign();
estimate_R_and_t(R, t);
return reprojection_error(R, t);
}
double upnp::reprojection_error(const double R[3][3], const double t[3])
{
double sum2 = 0.0;
for(int i = 0; i < number_of_correspondences; i++) {
double * pw = &pws[3 * i];
double Xc = dot(R[0], pw) + t[0];
double Yc = dot(R[1], pw) + t[1];
double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
double ue = uc + fu * Xc * inv_Zc;
double ve = vc + fv * Yc * inv_Zc;
double u = us[2 * i], v = us[2 * i + 1];
sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
}
return sum2 / number_of_correspondences;
}
void upnp::choose_control_points()
{
for (int i = 0; i < 4; ++i)
cws[i][0] = cws[i][1] = cws[i][2] = 0.0;
cws[0][0] = cws[1][1] = cws[2][2] = 1.0;
}
void upnp::compute_alphas()
{
Mat CC = Mat(4, 3, CV_64F, &cws);
Mat PC = Mat(number_of_correspondences, 3, CV_64F, &pws[0]);
Mat ALPHAS = Mat(number_of_correspondences, 4, CV_64F, &alphas[0]);
Mat CC_ = CC.clone().t();
Mat PC_ = PC.clone().t();
Mat row14 = Mat::ones(1, 4, CV_64F);
Mat row1n = Mat::ones(1, number_of_correspondences, CV_64F);
CC_.push_back(row14);
PC_.push_back(row1n);
ALPHAS = Mat( CC_.inv() * PC_ ).t();
}
void upnp::fill_M(Mat * M, const int row, const double * as, const double u, const double v)
{
double * M1 = M->ptr<double>(row);
double * M2 = M1 + 12;
for(int i = 0; i < 4; i++) {
M1[3 * i ] = as[i] * fu;
M1[3 * i + 1] = 0.0;
M1[3 * i + 2] = as[i] * (uc - u);
M2[3 * i ] = 0.0;
M2[3 * i + 1] = as[i] * fv;
M2[3 * i + 2] = as[i] * (vc - v);
}
}
void upnp::compute_ccs(const double * betas, const double * ut)
{
for(int i = 0; i < 4; ++i)
ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0;
int N = 4;
for(int i = 0; i < N; ++i) {
const double * v = ut + 12 * (9 + i);
for(int j = 0; j < 4; ++j)
for(int k = 0; k < 3; ++k)
ccs[j][k] += betas[i] * v[3 * j + k]; // FIXIT: array subscript 144 is outside array bounds of 'double [144]' [-Warray-bounds]
// line 109: double ut[12 * 12]
// line 359: double u[12*12]
}
for (int i = 0; i < 4; ++i) ccs[i][2] *= fu;
}
void upnp::compute_pcs(void)
{
for(int i = 0; i < number_of_correspondences; i++) {
double * a = &alphas[0] + 4 * i;
double * pc = &pcs[0] + 3 * i;
for(int j = 0; j < 3; j++)
pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];
}
}
void upnp::find_betas_and_focal_approx_1(Mat * Ut, Mat * Rho, double * betas, double * efs)
{
Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
Mat D = compute_constraint_distance_2param_6eq_2unk_f_unk( Kmf1 );
Mat Dt = D.t();
Mat A = Dt * D;
Mat b = Dt * dsq;
Mat x = Mat(2, 1, CV_64F);
solve(A, b, x);
betas[0] = sqrt( abs( x.at<double>(0) ) );
betas[1] = betas[2] = betas[3] = 0.0;
efs[0] = sqrt( abs( x.at<double>(1) ) ) / betas[0];
}
void upnp::find_betas_and_focal_approx_2(Mat * Ut, Mat * Rho, double * betas, double * efs)
{
double u[12*12];
Mat U = Mat(12, 12, CV_64F, u);
Ut->copyTo(U);
Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(10));
Mat Kmf2 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 );
Mat A = D;
Mat b = dsq;
double x[6];
Mat X = Mat(6, 1, CV_64F, x);
solve(A, b, X, DECOMP_QR);
double solutions[18][3];
generate_all_possible_solutions_for_f_unk(x, solutions);
// find solution with minimum reprojection error
double min_error = std::numeric_limits<double>::max();
int min_sol = 0;
for (int i = 0; i < 18; ++i) {
betas[3] = solutions[i][0];
betas[2] = solutions[i][1];
betas[1] = betas[0] = 0.0;
fu = fv = solutions[i][2];
double Rs[3][3], ts[3];
double error_i = compute_R_and_t( u, betas, Rs, ts);
if( error_i < min_error)
{
min_error = error_i;
min_sol = i;
}
}
betas[0] = solutions[min_sol][0];
betas[1] = solutions[min_sol][1];
betas[2] = betas[3] = 0.0;
efs[0] = solutions[min_sol][2];
}
Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const Mat& M1)
{
Mat P = Mat(6, 2, CV_64F);
double m[13];
for (int i = 1; i < 13; ++i) m[i] = *M1.ptr<double>(i-1);
double t1 = pow( m[4], 2 );
double t4 = pow( m[1], 2 );
double t5 = pow( m[5], 2 );
double t8 = pow( m[2], 2 );
double t10 = pow( m[6], 2 );
double t13 = pow( m[3], 2 );
double t15 = pow( m[7], 2 );
double t18 = pow( m[8], 2 );
double t22 = pow( m[9], 2 );
double t26 = pow( m[10], 2 );
double t29 = pow( m[11], 2 );
double t33 = pow( m[12], 2 );
*P.ptr<double>(0,0) = t1 - 2 * m[4] * m[1] + t4 + t5 - 2 * m[5] * m[2] + t8;
*P.ptr<double>(0,1) = t10 - 2 * m[6] * m[3] + t13;
*P.ptr<double>(1,0) = t15 - 2 * m[7] * m[1] + t4 + t18 - 2 * m[8] * m[2] + t8;
*P.ptr<double>(1,1) = t22 - 2 * m[9] * m[3] + t13;
*P.ptr<double>(2,0) = t26 - 2 * m[10] * m[1] + t4 + t29 - 2 * m[11] * m[2] + t8;
*P.ptr<double>(2,1) = t33 - 2 * m[12] * m[3] + t13;
*P.ptr<double>(3,0) = t15 - 2 * m[7] * m[4] + t1 + t18 - 2 * m[8] * m[5] + t5;
*P.ptr<double>(3,1) = t22 - 2 * m[9] * m[6] + t10;
*P.ptr<double>(4,0) = t26 - 2 * m[10] * m[4] + t1 + t29 - 2 * m[11] * m[5] + t5;
*P.ptr<double>(4,1) = t33 - 2 * m[12] * m[6] + t10;
*P.ptr<double>(5,0) = t26 - 2 * m[10] * m[7] + t15 + t29 - 2 * m[11] * m[8] + t18;
*P.ptr<double>(5,1) = t33 - 2 * m[12] * m[9] + t22;
return P;
}
Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(const Mat& M1, const Mat& M2)
{
Mat P = Mat(6, 6, CV_64F);
double m[3][13];
for (int i = 1; i < 13; ++i)
{
m[1][i] = *M1.ptr<double>(i-1);
m[2][i] = *M2.ptr<double>(i-1);
}
double t1 = pow( m[1][4], 2 );
double t2 = pow( m[1][1], 2 );
double t7 = pow( m[1][5], 2 );
double t8 = pow( m[1][2], 2 );
double t11 = m[1][1] * m[2][1];
double t12 = m[1][5] * m[2][5];
double t15 = m[1][2] * m[2][2];
double t16 = m[1][4] * m[2][4];
double t19 = pow( m[2][4], 2 );
double t22 = pow( m[2][2], 2 );
double t23 = pow( m[2][1], 2 );
double t24 = pow( m[2][5], 2 );
double t28 = pow( m[1][6], 2 );
double t29 = pow( m[1][3], 2 );
double t34 = pow( m[1][3], 2 );
double t36 = m[1][6] * m[2][6];
double t40 = pow( m[2][6], 2 );
double t41 = pow( m[2][3], 2 );
double t47 = pow( m[1][7], 2 );
double t48 = pow( m[1][8], 2 );
double t52 = m[1][7] * m[2][7];
double t55 = m[1][8] * m[2][8];
double t59 = pow( m[2][8], 2 );
double t62 = pow( m[2][7], 2 );
double t64 = pow( m[1][9], 2 );
double t68 = m[1][9] * m[2][9];
double t74 = pow( m[2][9], 2 );
double t78 = pow( m[1][10], 2 );
double t79 = pow( m[1][11], 2 );
double t84 = m[1][10] * m[2][10];
double t87 = m[1][11] * m[2][11];
double t90 = pow( m[2][10], 2 );
double t95 = pow( m[2][11], 2 );
double t99 = pow( m[1][12], 2 );
double t101 = m[1][12] * m[2][12];
double t105 = pow( m[2][12], 2 );
*P.ptr<double>(0,0) = t1 + t2 - 2 * m[1][4] * m[1][1] - 2 * m[1][5] * m[1][2] + t7 + t8;
*P.ptr<double>(0,1) = -2 * m[2][4] * m[1][1] + 2 * t11 + 2 * t12 - 2 * m[1][4] * m[2][1] - 2 * m[2][5] * m[1][2] + 2 * t15 + 2 * t16 - 2 * m[1][5] * m[2][2];
*P.ptr<double>(0,2) = t19 - 2 * m[2][4] * m[2][1] + t22 + t23 + t24 - 2 * m[2][5] * m[2][2];
*P.ptr<double>(0,3) = t28 + t29 - 2 * m[1][6] * m[1][3];
*P.ptr<double>(0,4) = -2 * m[2][6] * m[1][3] + 2 * t34 - 2 * m[1][6] * m[2][3] + 2 * t36;
*P.ptr<double>(0,5) = -2 * m[2][6] * m[2][3] + t40 + t41;
*P.ptr<double>(1,0) = t8 - 2 * m[1][8] * m[1][2] - 2 * m[1][7] * m[1][1] + t47 + t48 + t2;
*P.ptr<double>(1,1) = 2 * t15 - 2 * m[1][8] * m[2][2] - 2 * m[2][8] * m[1][2] + 2 * t52 - 2 * m[1][7] * m[2][1] - 2 * m[2][7] * m[1][1] + 2 * t55 + 2 * t11;
*P.ptr<double>(1,2) = -2 * m[2][8] * m[2][2] + t22 + t23 + t59 - 2 * m[2][7] * m[2][1] + t62;
*P.ptr<double>(1,3) = t29 + t64 - 2 * m[1][9] * m[1][3];
*P.ptr<double>(1,4) = 2 * t34 + 2 * t68 - 2 * m[2][9] * m[1][3] - 2 * m[1][9] * m[2][3];
*P.ptr<double>(1,5) = -2 * m[2][9] * m[2][3] + t74 + t41;
*P.ptr<double>(2,0) = -2 * m[1][11] * m[1][2] + t2 + t8 + t78 + t79 - 2 * m[1][10] * m[1][1];
*P.ptr<double>(2,1) = 2 * t15 - 2 * m[1][11] * m[2][2] + 2 * t84 - 2 * m[1][10] * m[2][1] - 2 * m[2][10] * m[1][1] + 2 * t87 - 2 * m[2][11] * m[1][2]+ 2 * t11;
*P.ptr<double>(2,2) = t90 + t22 - 2 * m[2][10] * m[2][1] + t23 - 2 * m[2][11] * m[2][2] + t95;
*P.ptr<double>(2,3) = -2 * m[1][12] * m[1][3] + t99 + t29;
*P.ptr<double>(2,4) = 2 * t34 + 2 * t101 - 2 * m[2][12] * m[1][3] - 2 * m[1][12] * m[2][3];
*P.ptr<double>(2,5) = t41 + t105 - 2 * m[2][12] * m[2][3];
*P.ptr<double>(3,0) = t48 + t1 - 2 * m[1][8] * m[1][5] + t7 - 2 * m[1][7] * m[1][4] + t47;
*P.ptr<double>(3,1) = 2 * t16 - 2 * m[1][7] * m[2][4] + 2 * t55 + 2 * t52 - 2 * m[1][8] * m[2][5] - 2 * m[2][8] * m[1][5] - 2 * m[2][7] * m[1][4] + 2 * t12;
*P.ptr<double>(3,2) = t24 - 2 * m[2][8] * m[2][5] + t19 - 2 * m[2][7] * m[2][4] + t62 + t59;
*P.ptr<double>(3,3) = -2 * m[1][9] * m[1][6] + t64 + t28;
*P.ptr<double>(3,4) = 2 * t68 + 2 * t36 - 2 * m[2][9] * m[1][6] - 2 * m[1][9] * m[2][6];
*P.ptr<double>(3,5) = t40 + t74 - 2 * m[2][9] * m[2][6];
*P.ptr<double>(4,0) = t1 - 2 * m[1][10] * m[1][4] + t7 + t78 + t79 - 2 * m[1][11] * m[1][5];
*P.ptr<double>(4,1) = 2 * t84 - 2 * m[1][11] * m[2][5] - 2 * m[1][10] * m[2][4] + 2 * t16 - 2 * m[2][11] * m[1][5] + 2 * t87 - 2 * m[2][10] * m[1][4] + 2 * t12;
*P.ptr<double>(4,2) = t19 + t24 - 2 * m[2][10] * m[2][4] - 2 * m[2][11] * m[2][5] + t95 + t90;
*P.ptr<double>(4,3) = t28 - 2 * m[1][12] * m[1][6] + t99;
*P.ptr<double>(4,4) = 2 * t101 + 2 * t36 - 2 * m[2][12] * m[1][6] - 2 * m[1][12] * m[2][6];
*P.ptr<double>(4,5) = t105 - 2 * m[2][12] * m[2][6] + t40;
*P.ptr<double>(5,0) = -2 * m[1][10] * m[1][7] + t47 + t48 + t78 + t79 - 2 * m[1][11] * m[1][8];
*P.ptr<double>(5,1) = 2 * t84 + 2 * t87 - 2 * m[2][11] * m[1][8] - 2 * m[1][10] * m[2][7] - 2 * m[2][10] * m[1][7] + 2 * t55 + 2 * t52 - 2 * m[1][11] * m[2][8];
*P.ptr<double>(5,2) = -2 * m[2][10] * m[2][7] - 2 * m[2][11] * m[2][8] + t62 + t59 + t90 + t95;
*P.ptr<double>(5,3) = t64 - 2 * m[1][12] * m[1][9] + t99;
*P.ptr<double>(5,4) = 2 * t68 - 2 * m[2][12] * m[1][9] - 2 * m[1][12] * m[2][9] + 2 * t101;
*P.ptr<double>(5,5) = t105 - 2 * m[2][12] * m[2][9] + t74;
return P;
}
void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
{
int matrix_to_resolve[18][9] = {
{ 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 },
{ 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 },
{ 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 },
{ 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 },
{ 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 },
{ 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
{ 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
{ 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 },
{ 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 }
};
int combination[18][3] = {
{ 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 },
{ 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 },
{ 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 },
{ 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 },
{ 3, 4, 6 }, { 3, 5, 6 }
};
for (int i = 0; i < 18; ++i) {
double matrix[9], independent_term[3];
Mat M = Mat(3, 3, CV_64F, matrix);
Mat I = Mat(3, 1, CV_64F, independent_term);
Mat S = Mat(1, 3, CV_64F);
for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];
independent_term[0] = log( abs( betas[ combination[i][0]-1 ] ) );
independent_term[1] = log( abs( betas[ combination[i][1]-1 ] ) );
independent_term[2] = log( abs( betas[ combination[i][2]-1 ] ) );
exp( Mat(M.inv() * I), S);
solutions[i][0] = S.at<double>(0);
solutions[i][1] = S.at<double>(1) * sign( betas[1] );
solutions[i][2] = abs( S.at<double>(2) );
}
}
void upnp::gauss_newton(const Mat * L_6x12, const Mat * Rho, double betas[4], double * f)
{
const int iterations_number = 50;
double a[6*4], b[6], x[4] = {0};
Mat * A = new Mat(6, 4, CV_64F, a);
Mat * B = new Mat(6, 1, CV_64F, b);
Mat * X = new Mat(4, 1, CV_64F, x);
for(int k = 0; k < iterations_number; k++)
{
compute_A_and_b_gauss_newton(L_6x12->ptr<double>(0), Rho->ptr<double>(0), betas, A, B, f[0]);
qr_solve(A, B, X);
for(int i = 0; i < 3; i++)
betas[i] += x[i];
f[0] += x[3];
}
if (f[0] < 0) f[0] = -f[0];
fu = fv = f[0];
A->release();
delete A;
B->release();
delete B;
X->release();
delete X;
}
void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
const double betas[4], Mat * A, Mat * b, double const f)
{
for(int i = 0; i < 6; i++) {
const double * rowL = l_6x12 + i * 12;
double * rowA = A->ptr<double>(i);
rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[2] * betas[2] + f*f * ( 2 * rowL[6]*betas[0] + rowL[7]*betas[1] + rowL[8]*betas[2] );
rowA[1] = rowL[1] * betas[0] + 2 * rowL[3] * betas[1] + rowL[4] * betas[2] + f*f * ( rowL[7]*betas[0] + 2 * rowL[9]*betas[1] + rowL[10]*betas[2] );
rowA[2] = rowL[2] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + f*f * ( rowL[8]*betas[0] + rowL[10]*betas[1] + 2 * rowL[11]*betas[2] );
rowA[3] = 2*f * ( rowL[6]*betas[0]*betas[0] + rowL[7]*betas[0]*betas[1] + rowL[8]*betas[0]*betas[2] + rowL[9]*betas[1]*betas[1] + rowL[10]*betas[1]*betas[2] + rowL[11]*betas[2]*betas[2] ) ;
*b->ptr<double>(i) = rho[i] -
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[0] * betas[2] +
rowL[3] * betas[1] * betas[1] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
f*f * rowL[6] * betas[0] * betas[0] +
f*f * rowL[7] * betas[0] * betas[1] +
f*f * rowL[8] * betas[0] * betas[2] +
f*f * rowL[9] * betas[1] * betas[1] +
f*f * rowL[10] * betas[1] * betas[2] +
f*f * rowL[11] * betas[2] * betas[2]
);
}
}
void upnp::compute_L_6x12(const double * ut, double * l_6x12)
{
const double * v[3];
v[0] = ut + 12 * 9;
v[1] = ut + 12 * 10;
v[2] = ut + 12 * 11;
double dv[3][6][3];
for(int i = 0; i < 3; i++) {
int a = 0, b = 1;
for(int j = 0; j < 6; j++) {
dv[i][j][0] = v[i][3 * a ] - v[i][3 * b];
dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
b++;
if (b > 3) {
a++;
b = a + 1;
}
}
}
for(int i = 0; i < 6; i++) {
double * row = l_6x12 + 12 * i;
row[0] = dotXY(dv[0][i], dv[0][i]);
row[1] = 2.0f * dotXY(dv[0][i], dv[1][i]);
row[2] = dotXY(dv[1][i], dv[1][i]);
row[3] = 2.0f * dotXY(dv[0][i], dv[2][i]);
row[4] = 2.0f * dotXY(dv[1][i], dv[2][i]);
row[5] = dotXY(dv[2][i], dv[2][i]);
row[6] = dotZ(dv[0][i], dv[0][i]);
row[7] = 2.0f * dotZ(dv[0][i], dv[1][i]);
row[8] = 2.0f * dotZ(dv[0][i], dv[2][i]);
row[9] = dotZ(dv[1][i], dv[1][i]);
row[10] = 2.0f * dotZ(dv[1][i], dv[2][i]);
row[11] = dotZ(dv[2][i], dv[2][i]);
}
}
void upnp::compute_rho(double * rho)
{
rho[0] = dist2(cws[0], cws[1]);
rho[1] = dist2(cws[0], cws[2]);
rho[2] = dist2(cws[0], cws[3]);
rho[3] = dist2(cws[1], cws[2]);
rho[4] = dist2(cws[1], cws[3]);
rho[5] = dist2(cws[2], cws[3]);
}
double upnp::dist2(const double * p1, const double * p2)
{
return
(p1[0] - p2[0]) * (p1[0] - p2[0]) +
(p1[1] - p2[1]) * (p1[1] - p2[1]) +
(p1[2] - p2[2]) * (p1[2] - p2[2]);
}
double upnp::dot(const double * v1, const double * v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
double upnp::dotXY(const double * v1, const double * v2)
{
return v1[0] * v2[0] + v1[1] * v2[1];
}
double upnp::dotZ(const double * v1, const double * v2)
{
return v1[2] * v2[2];
}
double upnp::sign(const double v)
{
return ( v < 0.0 ) ? -1.0 : ( v > 0.0 ) ? 1.0 : 0.0;
}
void upnp::qr_solve(Mat * A, Mat * b, Mat * X)
{
const int nr = A->rows;
const int nc = A->cols;
if (nr <= 0 || nc <= 0)
return;
if (max_nr != 0 && max_nr < nr)
{
delete [] A1;
delete [] A2;
}
if (max_nr < nr)
{
max_nr = nr;
A1 = new double[nr];
A2 = new double[nr];
}
double * pA = A->ptr<double>(0), * ppAkk = pA;
for(int k = 0; k < nc; k++)
{
double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
for(int i = k + 1; i < nr; i++)
{
double elt = fabs(*ppAik1);
if (eta < elt) eta = elt;
ppAik1 += nc;
}
if (eta == 0)
{
A1[k] = A2[k] = 0.0;
//cerr << "God damnit, A is singular, this shouldn't happen." << endl;
return;
}
else
{
double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++)
{
*ppAik2 *= inv_eta;
sum2 += *ppAik2 * *ppAik2;
ppAik2 += nc;
}
double sigma = sqrt(sum2);
if (*ppAkk < 0)
sigma = -sigma;
*ppAkk += sigma;
A1[k] = sigma * *ppAkk;
A2[k] = -eta * sigma;
for(int j = k + 1; j < nc; j++)
{
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++)
{
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++)
{
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
}
}
ppAkk += nc + 1;
}
// b <- Qt b
double * ppAjj = pA, * pb = b->ptr<double>(0);
for(int j = 0; j < nc; j++)
{
double * ppAij = ppAjj, tau = 0;
for(int i = j; i < nr; i++)
{
tau += *ppAij * pb[i];
ppAij += nc;
}
tau /= A1[j];
ppAij = ppAjj;
for(int i = j; i < nr; i++)
{
pb[i] -= tau * *ppAij;
ppAij += nc;
}
ppAjj += nc + 1;
}
// X = R-1 b
double * pX = X->ptr<double>(0);
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--)
{
double * ppAij = pA + i * nc + (i + 1), sum = 0;
for(int j = i + 1; j < nc; j++)
{
sum += *ppAij * pX[j];
ppAij++;
}
pX[i] = (pb[i] - sum) / A2[i];
}
}
#endif

140
Lib/opencv/sources/modules/calib3d/src/upnp.h Normal file
View File

@@ -0,0 +1,140 @@
//M*//////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
/****************************************************************************************\
* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
* Contributed by Edgar Riba
\****************************************************************************************/
#ifndef OPENCV_CALIB3D_UPNP_H_
#define OPENCV_CALIB3D_UPNP_H_
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
#include <iostream>
#if 0 // fix buffer overflow first (FIXIT mark in .cpp file)
class upnp
{
public:
upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
~upnp();
double compute_pose(cv::Mat& R, cv::Mat& t);
private:
upnp(const upnp &); // copy disabled
upnp& operator=(const upnp &); // assign disabled
template <typename T>
void init_camera_parameters(const cv::Mat& cameraMatrix)
{
uc = cameraMatrix.at<T> (0, 2);
vc = cameraMatrix.at<T> (1, 2);
fu = 1;
fv = 1;
}
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
for(int i = 0; i < number_of_correspondences; i++)
{
pws[3 * i ] = opoints.at<OpointType>(i).x;
pws[3 * i + 1] = opoints.at<OpointType>(i).y;
pws[3 * i + 2] = opoints.at<OpointType>(i).z;
us[2 * i ] = ipoints.at<IpointType>(i).x;
us[2 * i + 1] = ipoints.at<IpointType>(i).y;
}
}
double reprojection_error(const double R[3][3], const double t[3]);
void choose_control_points();
void compute_alphas();
void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v);
void compute_ccs(const double * betas, const double * ut);
void compute_pcs(void);
void solve_for_sign(void);
void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X);
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]);
double sign(const double v);
double dot(const double * v1, const double * v2);
double dotXY(const double * v1, const double * v2);
double dotZ(const double * v1, const double * v2);
double dist2(const double * p1, const double * p2);
void compute_rho(double * rho);
void compute_L_6x12(const double * ut, double * l_6x12);
void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs);
void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
const double cb[4], cv::Mat * A, cv::Mat * b, double const f);
double compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3]);
void estimate_R_and_t(double R[3][3], double t[3]);
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
double R_src[3][3], double t_src[3]);
double uc, vc, fu, fv;
std::vector<double> pws, us, alphas, pcs;
int number_of_correspondences;
double cws[4][3], ccs[4][3];
int max_nr;
double * A1, * A2;
};
#endif
#endif // OPENCV_CALIB3D_UPNP_H_