From 7829d27f38b441d832093d80d3cb16063476c2d7 Mon Sep 17 00:00:00 2001 From: danini Date: Mon, 7 Oct 2019 15:41:57 +0200 Subject: [PATCH] Adding 6D pose estimation --- CMakeLists.txt | 1 + include/model.h | 12 + include/perspective_n_point_estimator.h | 211 ++++++ include/solver_dls_pnp.h | 919 ++++++++++++++++++++++++ include/solver_p3p.h | 368 ++++++++++ include/types.h | 14 +- 6 files changed, 1522 insertions(+), 3 deletions(-) create mode 100644 include/perspective_n_point_estimator.h create mode 100644 include/solver_dls_pnp.h create mode 100644 include/solver_p3p.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6198559..8e6c7ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,7 @@ source_group(third_party\\GCOptimization FILES source_group(Models FILES include/solver_engine.h include/solver_essential_matrix_five_point_stewenius.h + include/solver_p3p.h include/solver_fundamental_matrix_eight_point.h include/solver_fundamental_matrix_seven_point.h include/solver_homography_four_point.h diff --git a/include/model.h b/include/model.h index edbe6bc..149e555 100644 --- a/include/model.h +++ b/include/model.h @@ -81,6 +81,18 @@ namespace gcransac } }; + class Pose6D : public Model + { + public: + Pose6D() : + Model(Eigen::MatrixXd(3, 4)) + {} + Pose6D(const Pose6D& other_) + { + descriptor = other_.descriptor; + } + }; + class Homography : public Model { public: diff --git a/include/perspective_n_point_estimator.h b/include/perspective_n_point_estimator.h new file mode 100644 index 0000000..6133652 --- /dev/null +++ b/include/perspective_n_point_estimator.h @@ -0,0 +1,211 @@ +// Copyright (C) 2019 Czech Technical University. +// 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 Czech Technical University 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 HOLDERS 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. +// +// Please contact the author of this library if you have any questions. +// Author: Daniel Barath (barath.daniel@sztaki.mta.hu) +#pragma once + +#define _USE_MATH_DEFINES + +#include +#include +#include +#include + +#include +#include + +#include "estimator.h" +#include "model.h" + +#include "solver_p3p.h" + +namespace gcransac +{ + namespace estimator + { + // This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented + template // The solver used for estimating the model from a non-minimal sample + class PerspectiveNPointEstimator : public Estimator < cv::Mat, Model > + { + protected: + // Minimal solver engine used for estimating a model from a minimal sample + const std::shared_ptr minimal_solver; + + // Non-minimal solver engine used for estimating a model from a bigger than minimal sample + const std::shared_ptr non_minimal_solver; + + // The lower bound of the inlier ratio which is required to pass the validity test. + // The validity test measures what proportion of the inlier (by Sampson distance) is inlier + // when using symmetric epipolar distance. + const double minimum_inlier_ratio_in_validity_check; + + public: + PerspectiveNPointEstimator(const double minimum_inlier_ratio_in_validity_check_ = 0.5) : + // Minimal solver engine used for estimating a model from a minimal sample + minimal_solver(std::make_shared()), + // Non-minimal solver engine used for estimating a model from a bigger than minimal sample + non_minimal_solver(std::make_shared()), + // The lower bound of the inlier ratio which is required to pass the validity test. + // It is clamped to be in interval [0, 1]. + minimum_inlier_ratio_in_validity_check(std::clamp(minimum_inlier_ratio_in_validity_check_, 0.0, 1.0)) + {} + + ~PerspectiveNPointEstimator() {} + + // The size of a non-minimal sample required for the estimation + static constexpr size_t nonMinimalSampleSize() { + return _NonMinimalSolverEngine::sampleSize(); + } + + // The size of a minimal sample required for the estimation + static constexpr size_t sampleSize() { + return _MinimalSolverEngine::sampleSize(); + } + + // The size of a sample when doing inner RANSAC on a non-minimal sample + inline size_t inlierLimit() const { + return 7 * sampleSize(); + } + + inline bool estimateModel(const cv::Mat& data, + const size_t *sample, + std::vector* models) const + { + // Estimate the model parameters by the minimal solver + minimal_solver->estimateModel(data, + sample, + sampleSize(), + *models); + + // The estimation was successfull if at least one model is kept + return models->size() > 0; + } + + inline double squaredReprojectionError(const cv::Mat& point_, + const Eigen::Matrix& descriptor_) const + { + const double* s = reinterpret_cast(point_.data); + const double u = *s, + v = *(s + 1), + x = *(s + 2), + y = *(s + 3), + z = *(s + 4); + + const double + r11 = descriptor_(0, 0), + r12 = descriptor_(0, 1), + r13 = descriptor_(0, 2), + r21 = descriptor_(1, 0), + r22 = descriptor_(1, 1), + r23 = descriptor_(1, 2), + r31 = descriptor_(2, 0), + r32 = descriptor_(2, 1), + r33 = descriptor_(2, 2), + tx = descriptor_(0, 3), + ty = descriptor_(1, 3), + tz = descriptor_(2, 3); + + const double px = r11 * x + r12 * y + r13 * z + tx, + py = r21 * x + r22 * y + r23 * z + ty, + pz = r31 * x + r32 * y + r33 * z + tz; + + const double pu = px / pz, + pv = py / pz; + + const double du = pu - u, + dv = pv - v; + + return du * du + dv * dv; + } + + inline double squaredResidual(const cv::Mat& point_, + const Model& model_) const + { + return squaredResidual(point_, model_.descriptor); + } + + inline double squaredResidual(const cv::Mat& point_, + const Eigen::MatrixXd& descriptor_) const + { + if (descriptor_.cols() != 4 || descriptor_.rows() != 3) + { + fprintf(stderr, "Error while calculating the residuals. " + "The size of the matrix should be 3*4 instead of %d*%d.\n", + descriptor_.rows(), descriptor_.cols()); + return 0.0; + } + return squaredReprojectionError(point_, descriptor_); + } + + inline double residual(const cv::Mat& point_, + const Model& model_) const + { + return residual(point_, model_.descriptor); + } + + inline double residual(const cv::Mat& point_, + const Eigen::MatrixXd& descriptor_) const + { + return sqrt(squaredReprojectionError(point_, descriptor_)); + } + + inline bool isValidModel(const Model& model_, + const cv::Mat& data_, + const std::vector &inliers_, + const double threshold_) const + { + return true; + } + + inline bool estimateModelNonminimal( + const cv::Mat& data_, + const size_t *sample_, + const size_t &sample_number_, + std::vector* models_, + const double *weights_ = nullptr) const + { + if (sample_number_ < nonMinimalSampleSize()) + return false; + + // The eight point fundamental matrix fitting algorithm + non_minimal_solver->estimateModel(data_, + sample_, + sample_number_, + *models_, + weights_); + + return true; + } + }; + } +} \ No newline at end of file diff --git a/include/solver_dls_pnp.h b/include/solver_dls_pnp.h new file mode 100644 index 0000000..40ceac0 --- /dev/null +++ b/include/solver_dls_pnp.h @@ -0,0 +1,919 @@ +// Copyright (C) 2019 Czech Technical University. +// 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 Czech Technical University 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 HOLDERS 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. +// +// Please contact the author of this library if you have any questions. +// Author: Daniel Barath (barath.daniel@sztaki.mta.hu) +#pragma once + +#include "solver_engine.h" +#include "fundamental_estimator.h" +#include + +namespace gcransac +{ + namespace estimator + { + namespace solver + { + // This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented + class DLSPnP : public SolverEngine + { + public: + DLSPnP() + { + } + + ~DLSPnP() + { + } + + // Determines if there is a chance of returning multiple models + // the function 'estimateModel' is applied. + static constexpr bool returnMultipleModels() + { + return true; + } + + // The minimum number of points required for the estimation + static constexpr size_t sampleSize() + { + return 3; + } + + // Estimate the model parameters from the given point sample + // using weighted fitting if possible. + inline bool estimateModel( + const cv::Mat& data_, // The set of data points + const size_t *sample_, // The sample used for the estimation + size_t sample_number_, // The size of the sample + std::vector &models_, // The estimated model parameters + const double *weights_ = nullptr) const; // The weight for each point + + protected: + void createMacaulayMatrix( + const double a[20], + const double b[20], + const double c[20], + const double u[4], + Eigen::MatrixXd &macaulay_matrix) const; + + // Transforms a 3 - vector in a 3x9 matrix such that : + // R * v = leftMultiplyMatrix(v) * vec(R) + // Where R is a rotation matrix and vec(R) converts R to a 9x1 vector. + Eigen::Matrix leftMultiplyMatrix(const Eigen::Vector3d& v) const; + + // Extracts the coefficients of the Jacobians of the LS cost function (which is + // parameterized by the 3 rotation coefficients s1, s2, s3). + void extractJacobianCoefficients( + const Eigen::Matrix& D, + double f1_coeff[20], + double f2_coeff[20], + double f3_coeff[20]) const; + }; + + + // Estimate the model parameters from the given point sample + // using weighted fitting if possible. + inline bool DLSPnP::estimateModel( + const cv::Mat& data_, // The set of data points + const size_t *sample_, // The sample used for the estimation + size_t sample_number_, // The size of the sample + std::vector &models_, // The estimated model parameters + const double *weights_) const // The weight for each point + { + if (sample_ == nullptr) + sample_number_ = data_.rows; + + if (sample_number_ < sampleSize()) + return false; + + const double * data_ptr = reinterpret_cast(data_.data); + const size_t columns = data_.cols; + + // Holds the normalized feature positions cross multiplied with itself + // i.e. n * n^t. This value is used multiple times so it is efficient to + // pre-compute it. + std::vector normalized_feature_cross; + std::vector world_points(sample_number_); + normalized_feature_cross.reserve(sample_number_); + for (int i = 0; i < sample_number_; i++) { + const size_t idx = columns * (sample_ == nullptr ? + i : sample_[i]); + + Eigen::Vector3d normalized_feature_pos; + normalized_feature_pos << + data_ptr[idx], data_ptr[idx + 1], 1.0; + normalized_feature_pos.normalize(); + + normalized_feature_cross.push_back(normalized_feature_pos * + normalized_feature_pos.transpose()); + + world_points[i] << + data_ptr[idx + 2], data_ptr[idx + 3], data_ptr[idx + 4]; + } + + // The bottom-right symmetric block matrix of inverse(A^T * A). Matrix H from + // Eq. 25 in the Appendix of the DLS paper. + Eigen::Matrix3d h_inverse = sample_number_ * Eigen::Matrix3d::Identity(); + for (int i = 0; i < sample_number_; i++) { + h_inverse = h_inverse - normalized_feature_cross[i]; + } + const Eigen::Matrix3d h_matrix = h_inverse.inverse(); + + // Compute V*W*b with the rotation parameters factored out. This is the + // translation parameterized by the 9 entries of the rotation matrix. + Eigen::Matrix translation_factor = + Eigen::Matrix::Zero(); + for (int i = 0; i < sample_number_; i++) { + translation_factor = translation_factor + + (normalized_feature_cross[i] - Eigen::Matrix3d::Identity()) * + leftMultiplyMatrix(world_points[i]); + } + + translation_factor = h_matrix * translation_factor; + + // Compute the cost function J' of Eq. 17 in DLS paper. This is a factorized + // version where the rotation matrix parameters have been pulled out. The + // entries to this equation are the coefficients to the cost function which is + // a quartic in the rotation parameters. + Eigen::Matrix ls_cost_coefficients = + Eigen::Matrix::Zero(); + for (int i = 0; i < sample_number_; i++) { + ls_cost_coefficients = + ls_cost_coefficients + + (leftMultiplyMatrix(world_points[i]) + translation_factor).transpose() * + (Eigen::Matrix3d::Identity() - normalized_feature_cross[i]) * + (leftMultiplyMatrix(world_points[i]) + translation_factor); + } + + // Extract the coefficients of the jacobian (Eq. 18) from the + // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the + // rotation parameters. Each entry of the jacobian will be 0 at the roots of + // the polynomial, so we can arrange a system of polynomials from these + // equations. + double f1_coeff[20]; + double f2_coeff[20]; + double f3_coeff[20]; + extractJacobianCoefficients(ls_cost_coefficients, f1_coeff, f2_coeff, + f3_coeff); + + // We create one equation with random terms that is generally non-zero at the + // roots of our system. + const Eigen::Vector4d rand_vec = 100.0 * Eigen::Vector4d::Random(); + const double macaulay_term[4] = { rand_vec(0), + rand_vec(1), + rand_vec(2), + rand_vec(3) }; + + // Create Macaulay matrix that will be used to solve our polynonomial system. + Eigen::MatrixXd macaulay_matrix; + createMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term, macaulay_matrix); + + // Via the Schur complement trick, the top-left of the Macaulay matrix + // contains a multiplication matrix whose eigenvectors correspond to solutions + // to our system of equations. + const Eigen::MatrixXd solution_polynomial = + macaulay_matrix.block<27, 27>(0, 0) - + (macaulay_matrix.block<27, 93>(0, 27) * + macaulay_matrix.block<93, 93>(27, 27).partialPivLu().solve( + macaulay_matrix.block<93, 27>(27, 0))); + + // Extract eigenvectors of the solution polynomial to obtain the roots which + // are contained in the entries of the eigenvectors. + const Eigen::EigenSolver eigen_solver(solution_polynomial); + + // Many of the eigenvectors will contain complex solutions so we must filter + // them to find the real solutions. + const auto eigen_vectors = eigen_solver.eigenvectors(); + for (int i = 0; i < 27; i++) { + // The first entry of the eigenvector should equal 1 according to our + // polynomial, so we must divide each solution by the first entry. + std::complex s1 = eigen_vectors(9, i) / eigen_vectors(0, i); + std::complex s2 = eigen_vectors(3, i) / eigen_vectors(0, i); + std::complex s3 = eigen_vectors(1, i) / eigen_vectors(0, i); + + // If the rotation solutions are real, treat this as a valid candidate + // rotation. + const double kEpsilon = 1e-6; + if (fabs(s1.imag()) < kEpsilon && fabs(s2.imag()) < kEpsilon && + fabs(s3.imag()) < kEpsilon) { + // Compute the rotation (which is the transpose rotation of our solution) + // and translation. + Eigen::Quaterniond soln_rotation(1.0, s1.real(), s2.real(), s3.real()); + soln_rotation = soln_rotation.inverse().normalized(); + + const Eigen::Matrix3d rot_mat = soln_rotation.inverse().toRotationMatrix(); + const Eigen::Map > rot_vec(rot_mat.data()); + const Eigen::Vector3d soln_translation = translation_factor * rot_vec; + + // Check that all points are in front of the camera. Discard the solution + // if this is not the case. + bool all_points_in_front_of_camera = true; + for (int j = 0; j < sample_number_; j++) { + const Eigen::Vector3d transformed_point = + soln_rotation * world_points[j] + soln_translation; + if (transformed_point.z() < 0) { + all_points_in_front_of_camera = false; + break; + } + } + + if (all_points_in_front_of_camera) { + + Model model; + model.descriptor = Eigen::Matrix(); + model.descriptor << soln_rotation.toRotationMatrix(), soln_translation; + models_.emplace_back(model); + } + } + } + } + + // Transforms a 3 - vector in a 3x9 matrix such that : + // R * v = leftMultiplyMatrix(v) * vec(R) + // Where R is a rotation matrix and vec(R) converts R to a 9x1 vector. + Eigen::Matrix DLSPnP::leftMultiplyMatrix(const Eigen::Vector3d& v) const + { + Eigen::Matrix left_mult_mat = Eigen::Matrix::Zero(); + left_mult_mat.block<1, 3>(0, 0) = v.transpose(); + left_mult_mat.block<1, 3>(1, 3) = v.transpose(); + left_mult_mat.block<1, 3>(2, 6) = v.transpose(); + return left_mult_mat; + } + + // Extracts the coefficients of the Jacobians of the LS cost function (which is + // parameterized by the 3 rotation coefficients s1, s2, s3). + void DLSPnP::extractJacobianCoefficients( + const Eigen::Matrix& D, + double f1_coeff[20], + double f2_coeff[20], + double f3_coeff[20]) const + { + f1_coeff[0] = + 2 * D(0, 5) - 2 * D(0, 7) + 2 * D(4, 5) - 2 * D(4, 7) + 2 * D(5, 0) + + 2 * D(5, 4) + 2 * D(5, 8) - 2 * D(7, 0) - 2 * D(7, 4) - 2 * D(7, 8) + + 2 * D(8, 5) - 2 * D(8, 7); // constant term + f1_coeff[1] = + (6 * D(0, 1) + 6 * D(0, 3) + 6 * D(1, 0) - 6 * D(1, 4) - 6 * D(1, 8) + + 6 * D(3, 0) - 6 * D(3, 4) - 6 * D(3, 8) - 6 * D(4, 1) - 6 * D(4, 3) - + 6 * D(8, 1) - 6 * D(8, 3)); // s1^2 * s2 + f1_coeff[2] = + (4 * D(0, 6) - 4 * D(0, 2) + 8 * D(1, 5) - 8 * D(1, 7) - 4 * D(2, 0) + + 4 * D(2, 4) + 4 * D(2, 8) + 8 * D(3, 5) - 8 * D(3, 7) + 4 * D(4, 2) - + 4 * D(4, 6) + 8 * D(5, 1) + 8 * D(5, 3) + 4 * D(6, 0) - 4 * D(6, 4) - + 4 * D(6, 8) - 8 * D(7, 1) - 8 * D(7, 3) + 4 * D(8, 2) - + 4 * D(8, 6)); // s1 * s2 + f1_coeff[3] = + (4 * D(0, 1) - 4 * D(0, 3) + 4 * D(1, 0) - 4 * D(1, 4) - 4 * D(1, 8) + + 8 * D(2, 5) - 8 * D(2, 7) - 4 * D(3, 0) + 4 * D(3, 4) + 4 * D(3, 8) - + 4 * D(4, 1) + 4 * D(4, 3) + 8 * D(5, 2) + 8 * D(5, 6) + 8 * D(6, 5) - + 8 * D(6, 7) - 8 * D(7, 2) - 8 * D(7, 6) - 4 * D(8, 1) + + 4 * D(8, 3)); // s1 * s3 + f1_coeff[4] = (8 * D(1, 1) - 8 * D(2, 2) - 8 * D(3, 3) + 8 * D(5, 5) + + 8 * D(6, 6) - 8 * D(7, 7)); // s2 * s3 + f1_coeff[5] = + (4 * D(1, 5) - 2 * D(0, 6) - 2 * D(0, 2) + 4 * D(1, 7) - 2 * D(2, 0) + + 2 * D(2, 4) - 2 * D(2, 8) + 4 * D(3, 5) + 4 * D(3, 7) + 2 * D(4, 2) + + 2 * D(4, 6) + 4 * D(5, 1) + 4 * D(5, 3) - 2 * D(6, 0) + 2 * D(6, 4) - + 2 * D(6, 8) + 4 * D(7, 1) + 4 * D(7, 3) - 2 * D(8, 2) - + 2 * D(8, 6)); // s2^2 * s3 + f1_coeff[6] = (2 * D(1, 4) - 2 * D(0, 3) - 2 * D(1, 0) - 2 * D(0, 1) - + 2 * D(1, 8) - 2 * D(3, 0) + 2 * D(3, 4) - 2 * D(3, 8) + + 2 * D(4, 1) + 2 * D(4, 3) - 2 * D(8, 1) - 2 * D(8, 3)); // s2^3 + f1_coeff[7] = + (4 * D(0, 8) - 4 * D(0, 0) + 8 * D(2, 2) + 8 * D(2, 6) + 4 * D(4, 4) + + 8 * D(6, 2) + 8 * D(6, 6) + 4 * D(8, 0) - 4 * D(8, 8)); // s1 * s3^2 + f1_coeff[8] = + (4 * D(0, 0) - 4 * D(4, 4) - 4 * D(4, 8) + 8 * D(5, 5) - 8 * D(5, 7) - + 8 * D(7, 5) + 8 * D(7, 7) - 4 * D(8, 4) - 4 * D(8, 8)); // s1 + f1_coeff[9] = (2 * D(0, 2) + 2 * D(0, 6) + 4 * D(1, 5) - 4 * D(1, 7) + + 2 * D(2, 0) + 2 * D(2, 4) + 2 * D(2, 8) - 4 * D(3, 5) + + 4 * D(3, 7) + 2 * D(4, 2) + 2 * D(4, 6) + 4 * D(5, 1) - + 4 * D(5, 3) + 2 * D(6, 0) + 2 * D(6, 4) + 2 * D(6, 8) - + 4 * D(7, 1) + 4 * D(7, 3) + 2 * D(8, 2) + 2 * D(8, 6)); // s3 + f1_coeff[10] = (2 * D(0, 1) + 2 * D(0, 3) + 2 * D(1, 0) + 2 * D(1, 4) + + 2 * D(1, 8) - 4 * D(2, 5) + 4 * D(2, 7) + 2 * D(3, 0) + + 2 * D(3, 4) + 2 * D(3, 8) + 2 * D(4, 1) + 2 * D(4, 3) - + 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) - 4 * D(6, 7) + + 4 * D(7, 2) - 4 * D(7, 6) + 2 * D(8, 1) + 2 * D(8, 3)); // s2 + f1_coeff[11] = + (2 * D(1, 8) - 2 * D(0, 3) - 2 * D(1, 0) - 2 * D(1, 4) - 2 * D(0, 1) + + 4 * D(2, 5) + 4 * D(2, 7) - 2 * D(3, 0) - 2 * D(3, 4) + 2 * D(3, 8) - + 2 * D(4, 1) - 2 * D(4, 3) + 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) + + 4 * D(6, 7) + 4 * D(7, 2) + 4 * D(7, 6) + 2 * D(8, 1) + + 2 * D(8, 3)); // s2 * s3^2 + f1_coeff[12] = + (6 * D(0, 5) - 6 * D(0, 7) - 6 * D(4, 5) + 6 * D(4, 7) + 6 * D(5, 0) - + 6 * D(5, 4) - 6 * D(5, 8) - 6 * D(7, 0) + 6 * D(7, 4) + 6 * D(7, 8) - + 6 * D(8, 5) + 6 * D(8, 7)); // s1^2 + f1_coeff[13] = + (2 * D(0, 7) - 2 * D(0, 5) + 4 * D(1, 2) + 4 * D(1, 6) + 4 * D(2, 1) - + 4 * D(2, 3) - 4 * D(3, 2) - 4 * D(3, 6) - 2 * D(4, 5) + 2 * D(4, 7) - + 2 * D(5, 0) - 2 * D(5, 4) + 2 * D(5, 8) + 4 * D(6, 1) - 4 * D(6, 3) + + 2 * D(7, 0) + 2 * D(7, 4) - 2 * D(7, 8) + 2 * D(8, 5) - + 2 * D(8, 7)); // s3^2 + f1_coeff[14] = + (2 * D(0, 7) - 2 * D(0, 5) - 4 * D(1, 2) + 4 * D(1, 6) - 4 * D(2, 1) - + 4 * D(2, 3) - 4 * D(3, 2) + 4 * D(3, 6) + 2 * D(4, 5) - 2 * D(4, 7) - + 2 * D(5, 0) + 2 * D(5, 4) - 2 * D(5, 8) + 4 * D(6, 1) + 4 * D(6, 3) + + 2 * D(7, 0) - 2 * D(7, 4) + 2 * D(7, 8) - 2 * D(8, 5) + + 2 * D(8, 7)); // s2^2 + f1_coeff[15] = + (2 * D(2, 8) - 2 * D(0, 6) - 2 * D(2, 0) - 2 * D(2, 4) - 2 * D(0, 2) - + 2 * D(4, 2) - 2 * D(4, 6) - 2 * D(6, 0) - 2 * D(6, 4) + 2 * D(6, 8) + + 2 * D(8, 2) + 2 * D(8, 6)); // s3^3 + f1_coeff[16] = + (4 * D(0, 5) + 4 * D(0, 7) + 8 * D(1, 2) + 8 * D(1, 6) + 8 * D(2, 1) + + 8 * D(2, 3) + 8 * D(3, 2) + 8 * D(3, 6) - 4 * D(4, 5) - 4 * D(4, 7) + + 4 * D(5, 0) - 4 * D(5, 4) - 4 * D(5, 8) + 8 * D(6, 1) + 8 * D(6, 3) + + 4 * D(7, 0) - 4 * D(7, 4) - 4 * D(7, 8) - 4 * D(8, 5) - + 4 * D(8, 7)); // s1 * s2 * s3 + f1_coeff[17] = + (4 * D(0, 4) - 4 * D(0, 0) + 8 * D(1, 1) + 8 * D(1, 3) + 8 * D(3, 1) + + 8 * D(3, 3) + 4 * D(4, 0) - 4 * D(4, 4) + 4 * D(8, 8)); // s1 * s2^2 + f1_coeff[18] = + (6 * D(0, 2) + 6 * D(0, 6) + 6 * D(2, 0) - 6 * D(2, 4) - 6 * D(2, 8) - + 6 * D(4, 2) - 6 * D(4, 6) + 6 * D(6, 0) - 6 * D(6, 4) - 6 * D(6, 8) - + 6 * D(8, 2) - 6 * D(8, 6)); // s1^2 * s3 + f1_coeff[19] = + (4 * D(0, 0) - 4 * D(0, 4) - 4 * D(0, 8) - 4 * D(4, 0) + 4 * D(4, 4) + + 4 * D(4, 8) - 4 * D(8, 0) + 4 * D(8, 4) + 4 * D(8, 8)); // s1^3 + + f2_coeff[0] = + -2 * D(0, 2) + 2 * D(0, 6) - 2 * D(2, 0) - 2 * D(2, 4) - 2 * D(2, 8) - + 2 * D(4, 2) + 2 * D(4, 6) + 2 * D(6, 0) + 2 * D(6, 4) + 2 * D(6, 8) - + 2 * D(8, 2) + 2 * D(8, 6); // constant term + f2_coeff[1] = + (4 * D(0, 4) - 4 * D(0, 0) + 8 * D(1, 1) + 8 * D(1, 3) + 8 * D(3, 1) + + 8 * D(3, 3) + 4 * D(4, 0) - 4 * D(4, 4) + 4 * D(8, 8)); // s1^2 * s2 + f2_coeff[2] = + (4 * D(0, 7) - 4 * D(0, 5) - 8 * D(1, 2) + 8 * D(1, 6) - 8 * D(2, 1) - + 8 * D(2, 3) - 8 * D(3, 2) + 8 * D(3, 6) + 4 * D(4, 5) - 4 * D(4, 7) - + 4 * D(5, 0) + 4 * D(5, 4) - 4 * D(5, 8) + 8 * D(6, 1) + 8 * D(6, 3) + + 4 * D(7, 0) - 4 * D(7, 4) + 4 * D(7, 8) - 4 * D(8, 5) + + 4 * D(8, 7)); // s1 * s2 + f2_coeff[3] = (8 * D(1, 1) - 8 * D(2, 2) - 8 * D(3, 3) + 8 * D(5, 5) + + 8 * D(6, 6) - 8 * D(7, 7)); // s1 * s3 + f2_coeff[4] = + (4 * D(0, 3) - 4 * D(0, 1) - 4 * D(1, 0) + 4 * D(1, 4) - 4 * D(1, 8) - + 8 * D(2, 5) - 8 * D(2, 7) + 4 * D(3, 0) - 4 * D(3, 4) + 4 * D(3, 8) + + 4 * D(4, 1) - 4 * D(4, 3) - 8 * D(5, 2) + 8 * D(5, 6) + 8 * D(6, 5) + + 8 * D(6, 7) - 8 * D(7, 2) + 8 * D(7, 6) - 4 * D(8, 1) + + 4 * D(8, 3)); // s2 * s3 + f2_coeff[5] = + (6 * D(4, 5) - 6 * D(0, 7) - 6 * D(0, 5) + 6 * D(4, 7) - 6 * D(5, 0) + + 6 * D(5, 4) - 6 * D(5, 8) - 6 * D(7, 0) + 6 * D(7, 4) - 6 * D(7, 8) - + 6 * D(8, 5) - 6 * D(8, 7)); // s2^2 * s3 + f2_coeff[6] = + (4 * D(0, 0) - 4 * D(0, 4) + 4 * D(0, 8) - 4 * D(4, 0) + 4 * D(4, 4) - + 4 * D(4, 8) + 4 * D(8, 0) - 4 * D(8, 4) + 4 * D(8, 8)); // s2^3 + f2_coeff[7] = + (2 * D(1, 8) - 2 * D(0, 3) - 2 * D(1, 0) - 2 * D(1, 4) - 2 * D(0, 1) + + 4 * D(2, 5) + 4 * D(2, 7) - 2 * D(3, 0) - 2 * D(3, 4) + 2 * D(3, 8) - + 2 * D(4, 1) - 2 * D(4, 3) + 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) + + 4 * D(6, 7) + 4 * D(7, 2) + 4 * D(7, 6) + 2 * D(8, 1) + + 2 * D(8, 3)); // s1 * s3^2 + f2_coeff[8] = (2 * D(0, 1) + 2 * D(0, 3) + 2 * D(1, 0) + 2 * D(1, 4) + + 2 * D(1, 8) - 4 * D(2, 5) + 4 * D(2, 7) + 2 * D(3, 0) + + 2 * D(3, 4) + 2 * D(3, 8) + 2 * D(4, 1) + 2 * D(4, 3) - + 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) - 4 * D(6, 7) + + 4 * D(7, 2) - 4 * D(7, 6) + 2 * D(8, 1) + 2 * D(8, 3)); // s1 + f2_coeff[9] = (2 * D(0, 5) + 2 * D(0, 7) - 4 * D(1, 2) + 4 * D(1, 6) - + 4 * D(2, 1) + 4 * D(2, 3) + 4 * D(3, 2) - 4 * D(3, 6) + + 2 * D(4, 5) + 2 * D(4, 7) + 2 * D(5, 0) + 2 * D(5, 4) + + 2 * D(5, 8) + 4 * D(6, 1) - 4 * D(6, 3) + 2 * D(7, 0) + + 2 * D(7, 4) + 2 * D(7, 8) + 2 * D(8, 5) + 2 * D(8, 7)); // s3 + f2_coeff[10] = + (8 * D(2, 2) - 4 * D(0, 8) - 4 * D(0, 0) - 8 * D(2, 6) + 4 * D(4, 4) - + 8 * D(6, 2) + 8 * D(6, 6) - 4 * D(8, 0) - 4 * D(8, 8)); // s2 + f2_coeff[11] = + (4 * D(0, 0) - 4 * D(4, 4) + 4 * D(4, 8) + 8 * D(5, 5) + 8 * D(5, 7) + + 8 * D(7, 5) + 8 * D(7, 7) + 4 * D(8, 4) - 4 * D(8, 8)); // s2 * s3^2 + f2_coeff[12] = + (2 * D(0, 6) - 2 * D(0, 2) + 4 * D(1, 5) - 4 * D(1, 7) - 2 * D(2, 0) + + 2 * D(2, 4) + 2 * D(2, 8) + 4 * D(3, 5) - 4 * D(3, 7) + 2 * D(4, 2) - + 2 * D(4, 6) + 4 * D(5, 1) + 4 * D(5, 3) + 2 * D(6, 0) - 2 * D(6, 4) - + 2 * D(6, 8) - 4 * D(7, 1) - 4 * D(7, 3) + 2 * D(8, 2) - + 2 * D(8, 6)); // s1^2 + f2_coeff[13] = + (2 * D(0, 2) - 2 * D(0, 6) + 4 * D(1, 5) + 4 * D(1, 7) + 2 * D(2, 0) + + 2 * D(2, 4) - 2 * D(2, 8) - 4 * D(3, 5) - 4 * D(3, 7) + 2 * D(4, 2) - + 2 * D(4, 6) + 4 * D(5, 1) - 4 * D(5, 3) - 2 * D(6, 0) - 2 * D(6, 4) + + 2 * D(6, 8) + 4 * D(7, 1) - 4 * D(7, 3) - 2 * D(8, 2) + + 2 * D(8, 6)); // s3^2 + f2_coeff[14] = + (6 * D(0, 2) - 6 * D(0, 6) + 6 * D(2, 0) - 6 * D(2, 4) + 6 * D(2, 8) - + 6 * D(4, 2) + 6 * D(4, 6) - 6 * D(6, 0) + 6 * D(6, 4) - 6 * D(6, 8) + + 6 * D(8, 2) - 6 * D(8, 6)); // s2^2 + f2_coeff[15] = + (2 * D(5, 8) - 2 * D(0, 7) - 2 * D(4, 5) - 2 * D(4, 7) - 2 * D(5, 0) - + 2 * D(5, 4) - 2 * D(0, 5) - 2 * D(7, 0) - 2 * D(7, 4) + 2 * D(7, 8) + + 2 * D(8, 5) + 2 * D(8, 7)); // s3^3 + f2_coeff[16] = + (8 * D(1, 5) - 4 * D(0, 6) - 4 * D(0, 2) + 8 * D(1, 7) - 4 * D(2, 0) + + 4 * D(2, 4) - 4 * D(2, 8) + 8 * D(3, 5) + 8 * D(3, 7) + 4 * D(4, 2) + + 4 * D(4, 6) + 8 * D(5, 1) + 8 * D(5, 3) - 4 * D(6, 0) + 4 * D(6, 4) - + 4 * D(6, 8) + 8 * D(7, 1) + 8 * D(7, 3) - 4 * D(8, 2) - + 4 * D(8, 6)); // s1 * s2 * s3 + f2_coeff[17] = + (6 * D(1, 4) - 6 * D(0, 3) - 6 * D(1, 0) - 6 * D(0, 1) - 6 * D(1, 8) - + 6 * D(3, 0) + 6 * D(3, 4) - 6 * D(3, 8) + 6 * D(4, 1) + 6 * D(4, 3) - + 6 * D(8, 1) - 6 * D(8, 3)); // s1 * s2^2 + f2_coeff[18] = + (2 * D(0, 5) + 2 * D(0, 7) + 4 * D(1, 2) + 4 * D(1, 6) + 4 * D(2, 1) + + 4 * D(2, 3) + 4 * D(3, 2) + 4 * D(3, 6) - 2 * D(4, 5) - 2 * D(4, 7) + + 2 * D(5, 0) - 2 * D(5, 4) - 2 * D(5, 8) + 4 * D(6, 1) + 4 * D(6, 3) + + 2 * D(7, 0) - 2 * D(7, 4) - 2 * D(7, 8) - 2 * D(8, 5) - + 2 * D(8, 7)); // s1^2 * s3 + f2_coeff[19] = + (2 * D(0, 1) + 2 * D(0, 3) + 2 * D(1, 0) - 2 * D(1, 4) - 2 * D(1, 8) + + 2 * D(3, 0) - 2 * D(3, 4) - 2 * D(3, 8) - 2 * D(4, 1) - 2 * D(4, 3) - + 2 * D(8, 1) - 2 * D(8, 3)); // s1^3 + + f3_coeff[0] = + 2 * D(0, 1) - 2 * D(0, 3) + 2 * D(1, 0) + 2 * D(1, 4) + 2 * D(1, 8) - + 2 * D(3, 0) - 2 * D(3, 4) - 2 * D(3, 8) + 2 * D(4, 1) - 2 * D(4, 3) + + 2 * D(8, 1) - 2 * D(8, 3); // constant term + f3_coeff[1] = + (2 * D(0, 5) + 2 * D(0, 7) + 4 * D(1, 2) + 4 * D(1, 6) + 4 * D(2, 1) + + 4 * D(2, 3) + 4 * D(3, 2) + 4 * D(3, 6) - 2 * D(4, 5) - 2 * D(4, 7) + + 2 * D(5, 0) - 2 * D(5, 4) - 2 * D(5, 8) + 4 * D(6, 1) + 4 * D(6, 3) + + 2 * D(7, 0) - 2 * D(7, 4) - 2 * D(7, 8) - 2 * D(8, 5) - + 2 * D(8, 7)); // s1^2 * s2 + f3_coeff[2] = (8 * D(1, 1) - 8 * D(2, 2) - 8 * D(3, 3) + 8 * D(5, 5) + + 8 * D(6, 6) - 8 * D(7, 7)); // s1 * s2 + f3_coeff[3] = + (4 * D(0, 7) - 4 * D(0, 5) + 8 * D(1, 2) + 8 * D(1, 6) + 8 * D(2, 1) - + 8 * D(2, 3) - 8 * D(3, 2) - 8 * D(3, 6) - 4 * D(4, 5) + 4 * D(4, 7) - + 4 * D(5, 0) - 4 * D(5, 4) + 4 * D(5, 8) + 8 * D(6, 1) - 8 * D(6, 3) + + 4 * D(7, 0) + 4 * D(7, 4) - 4 * D(7, 8) + 4 * D(8, 5) - + 4 * D(8, 7)); // s1 * s3 + f3_coeff[4] = + (4 * D(0, 2) - 4 * D(0, 6) + 8 * D(1, 5) + 8 * D(1, 7) + 4 * D(2, 0) + + 4 * D(2, 4) - 4 * D(2, 8) - 8 * D(3, 5) - 8 * D(3, 7) + 4 * D(4, 2) - + 4 * D(4, 6) + 8 * D(5, 1) - 8 * D(5, 3) - 4 * D(6, 0) - 4 * D(6, 4) + + 4 * D(6, 8) + 8 * D(7, 1) - 8 * D(7, 3) - 4 * D(8, 2) + + 4 * D(8, 6)); // s2 * s3 + f3_coeff[5] = + (4 * D(0, 0) - 4 * D(4, 4) + 4 * D(4, 8) + 8 * D(5, 5) + 8 * D(5, 7) + + 8 * D(7, 5) + 8 * D(7, 7) + 4 * D(8, 4) - 4 * D(8, 8)); // s2^2 * s3 + f3_coeff[6] = (2 * D(4, 5) - 2 * D(0, 7) - 2 * D(0, 5) + 2 * D(4, 7) - + 2 * D(5, 0) + 2 * D(5, 4) - 2 * D(5, 8) - 2 * D(7, 0) + + 2 * D(7, 4) - 2 * D(7, 8) - 2 * D(8, 5) - 2 * D(8, 7)); // s2^3 + f3_coeff[7] = + (6 * D(2, 8) - 6 * D(0, 6) - 6 * D(2, 0) - 6 * D(2, 4) - 6 * D(0, 2) - + 6 * D(4, 2) - 6 * D(4, 6) - 6 * D(6, 0) - 6 * D(6, 4) + 6 * D(6, 8) + + 6 * D(8, 2) + 6 * D(8, 6)); // s1 * s3^2 + f3_coeff[8] = (2 * D(0, 2) + 2 * D(0, 6) + 4 * D(1, 5) - 4 * D(1, 7) + + 2 * D(2, 0) + 2 * D(2, 4) + 2 * D(2, 8) - 4 * D(3, 5) + + 4 * D(3, 7) + 2 * D(4, 2) + 2 * D(4, 6) + 4 * D(5, 1) - + 4 * D(5, 3) + 2 * D(6, 0) + 2 * D(6, 4) + 2 * D(6, 8) - + 4 * D(7, 1) + 4 * D(7, 3) + 2 * D(8, 2) + 2 * D(8, 6)); // s1 + f3_coeff[9] = + (8 * D(1, 1) - 4 * D(0, 4) - 4 * D(0, 0) - 8 * D(1, 3) - 8 * D(3, 1) + + 8 * D(3, 3) - 4 * D(4, 0) - 4 * D(4, 4) + 4 * D(8, 8)); // s3 + f3_coeff[10] = (2 * D(0, 5) + 2 * D(0, 7) - 4 * D(1, 2) + 4 * D(1, 6) - + 4 * D(2, 1) + 4 * D(2, 3) + 4 * D(3, 2) - 4 * D(3, 6) + + 2 * D(4, 5) + 2 * D(4, 7) + 2 * D(5, 0) + 2 * D(5, 4) + + 2 * D(5, 8) + 4 * D(6, 1) - 4 * D(6, 3) + 2 * D(7, 0) + + 2 * D(7, 4) + 2 * D(7, 8) + 2 * D(8, 5) + 2 * D(8, 7)); // s2 + f3_coeff[11] = + (6 * D(5, 8) - 6 * D(0, 7) - 6 * D(4, 5) - 6 * D(4, 7) - 6 * D(5, 0) - + 6 * D(5, 4) - 6 * D(0, 5) - 6 * D(7, 0) - 6 * D(7, 4) + 6 * D(7, 8) + + 6 * D(8, 5) + 6 * D(8, 7)); // s2 * s3^2 + f3_coeff[12] = + (2 * D(0, 1) - 2 * D(0, 3) + 2 * D(1, 0) - 2 * D(1, 4) - 2 * D(1, 8) + + 4 * D(2, 5) - 4 * D(2, 7) - 2 * D(3, 0) + 2 * D(3, 4) + 2 * D(3, 8) - + 2 * D(4, 1) + 2 * D(4, 3) + 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) - + 4 * D(6, 7) - 4 * D(7, 2) - 4 * D(7, 6) - 2 * D(8, 1) + + 2 * D(8, 3)); // s1^2 + f3_coeff[13] = + (6 * D(0, 3) - 6 * D(0, 1) - 6 * D(1, 0) - 6 * D(1, 4) + 6 * D(1, 8) + + 6 * D(3, 0) + 6 * D(3, 4) - 6 * D(3, 8) - 6 * D(4, 1) + 6 * D(4, 3) + + 6 * D(8, 1) - 6 * D(8, 3)); // s3^2 + f3_coeff[14] = + (2 * D(0, 3) - 2 * D(0, 1) - 2 * D(1, 0) + 2 * D(1, 4) - 2 * D(1, 8) - + 4 * D(2, 5) - 4 * D(2, 7) + 2 * D(3, 0) - 2 * D(3, 4) + 2 * D(3, 8) + + 2 * D(4, 1) - 2 * D(4, 3) - 4 * D(5, 2) + 4 * D(5, 6) + 4 * D(6, 5) + + 4 * D(6, 7) - 4 * D(7, 2) + 4 * D(7, 6) - 2 * D(8, 1) + + 2 * D(8, 3)); // s2^2 + f3_coeff[15] = + (4 * D(0, 0) + 4 * D(0, 4) - 4 * D(0, 8) + 4 * D(4, 0) + 4 * D(4, 4) - + 4 * D(4, 8) - 4 * D(8, 0) - 4 * D(8, 4) + 4 * D(8, 8)); // s3^3 + f3_coeff[16] = + (4 * D(1, 8) - 4 * D(0, 3) - 4 * D(1, 0) - 4 * D(1, 4) - 4 * D(0, 1) + + 8 * D(2, 5) + 8 * D(2, 7) - 4 * D(3, 0) - 4 * D(3, 4) + 4 * D(3, 8) - + 4 * D(4, 1) - 4 * D(4, 3) + 8 * D(5, 2) + 8 * D(5, 6) + 8 * D(6, 5) + + 8 * D(6, 7) + 8 * D(7, 2) + 8 * D(7, 6) + 4 * D(8, 1) + + 4 * D(8, 3)); // s1 * s2 * s3 + f3_coeff[17] = + (4 * D(1, 5) - 2 * D(0, 6) - 2 * D(0, 2) + 4 * D(1, 7) - 2 * D(2, 0) + + 2 * D(2, 4) - 2 * D(2, 8) + 4 * D(3, 5) + 4 * D(3, 7) + 2 * D(4, 2) + + 2 * D(4, 6) + 4 * D(5, 1) + 4 * D(5, 3) - 2 * D(6, 0) + 2 * D(6, 4) - + 2 * D(6, 8) + 4 * D(7, 1) + 4 * D(7, 3) - 2 * D(8, 2) - + 2 * D(8, 6)); // s1 * s2^2 + f3_coeff[18] = + (4 * D(0, 8) - 4 * D(0, 0) + 8 * D(2, 2) + 8 * D(2, 6) + 4 * D(4, 4) + + 8 * D(6, 2) + 8 * D(6, 6) + 4 * D(8, 0) - 4 * D(8, 8)); // s1^2 * s3 + f3_coeff[19] = + (2 * D(0, 2) + 2 * D(0, 6) + 2 * D(2, 0) - 2 * D(2, 4) - 2 * D(2, 8) - + 2 * D(4, 2) - 2 * D(4, 6) + 2 * D(6, 0) - 2 * D(6, 4) - 2 * D(6, 8) - + 2 * D(8, 2) - 2 * D(8, 6)); // s1^3 + } + + void DLSPnP::createMacaulayMatrix(const double a[20], + const double b[20], + const double c[20], + const double u[4], + Eigen::MatrixXd &macaulay_matrix) const + { + macaulay_matrix = Eigen::MatrixXd(120, 120); + macaulay_matrix.setZero(); + + // The matrix is very large (14400 elements!) and sparse (1968 non-zero + // elements) so we load it from pre-computed values calculated in matlab. + + const static int indices[1968] = { + 0, 35, 83, 118, 120, 121, 154, 155, 174, 203, 219, 238, 241, 242, 274, 275, + 291, 294, 305, 323, 329, 339, 358, 360, 363, 395, 409, 436, 443, 478, 479, + 481, 483, 484, 514, 515, 523, 529, 534, 551, 556, 563, 579, 580, 598, 599, + 602, 604, 605, 634, 635, 641, 643, 649, 651, 654, 662, 665, 671, 676, 683, + 689, 699, 700, 711, 718, 719, 723, 726, 750, 755, 769, 795, 796, 803, 827, + 838, 839, 844, 846, 847, 870, 874, 875, 883, 885, 889, 894, 903, 911, 915, + 916, 923, 939, 940, 947, 952, 958, 959, 965, 967, 968, 990, 994, 1001, 1003, + 1005, 1006, 1009, 1011, 1014, 1022, 1023, 1025, 1026, 1031, 1035, 1036, + 1049, 1059, 1060, 1062, 1067, 1071, 1072, 1079, 1080, 1089, 1115, 1116, + 1163, 1164, 1168, 1198, 1201, 1209, 1210, 1233, 1234, 1235, 1236, 1254, + 1259, 1283, 1284, 1288, 1299, 1317, 1318, 1322, 1330, 1331, 1348, 1353, + 1354, 1355, 1356, 1371, 1374, 1377, 1379, 1385, 1403, 1404, 1408, 1409, + 1419, 1434, 1437, 1438, 1443, 1449, 1452, 1475, 1476, 1479, 1489, 1516, + 1519, 1523, 1524, 1528, 1536, 1558, 1559, 1564, 1570, 1572, 1573, 1593, + 1594, 1595, 1596, 1599, 1603, 1607, 1609, 1614, 1619, 1620, 1631, 1636, + 1639, 1643, 1644, 1648, 1650, 1656, 1659, 1660, 1677, 1678, 1679, 1685, + 1691, 1693, 1694, 1708, 1713, 1714, 1716, 1719, 1721, 1722, 1723, 1727, + 1729, 1731, 1734, 1736, 1737, 1739, 1740, 1742, 1745, 1751, 1756, 1759, + 1764, 1768, 1769, 1770, 1776, 1779, 1780, 1786, 1791, 1794, 1797, 1799, + 1806, 1812, 1815, 1829, 1830, 1835, 1836, 1839, 1849, 1874, 1875, 1876, + 1879, 1883, 1884, 1888, 1894, 1896, 1907, 1918, 1919, 1927, 1933, 1935, + 1936, 1949, 1950, 1953, 1954, 1956, 1959, 1963, 1964, 1965, 1967, 1969, + 1974, 1979, 1980, 1983, 1988, 1991, 1994, 1995, 1996, 1999, 2004, 2008, + 2010, 2014, 2016, 2017, 2019, 2020, 2027, 2032, 2037, 2039, 2048, 2054, + 2056, 2057, 2068, 2069, 2070, 2073, 2079, 2081, 2082, 2083, 2084, 2085, + 2086, 2087, 2091, 2096, 2097, 2099, 2100, 2102, 2103, 2105, 2106, 2108, + 2111, 2114, 2115, 2119, 2129, 2130, 2134, 2136, 2137, 2140, 2142, 2146, + 2147, 2151, 2152, 2154, 2157, 2169, 2178, 2195, 2196, 2213, 2242, 2243, + 2244, 2247, 2248, 2278, 2290, 2298, 2299, 2312, 2313, 2314, 2315, 2316, + 2333, 2334, 2339, 2341, 2362, 2363, 2364, 2367, 2368, 2379, 2396, 2397, + 2398, 2411, 2419, 2420, 2427, 2428, 2432, 2433, 2434, 2436, 2451, 2453, + 2454, 2455, 2457, 2459, 2461, 2465, 2482, 2484, 2487, 2488, 2489, 2499, + 2513, 2514, 2516, 2517, 2532, 2538, 2541, 2555, 2556, 2558, 2559, 2569, + 2573, 2596, 2598, 2599, 2602, 2603, 2604, 2607, 2608, 2612, 2616, 2638, + 2639, 2653, 2659, 2661, 2662, 2672, 2673, 2674, 2676, 2678, 2679, 2680, + 2683, 2687, 2689, 2693, 2694, 2699, 2700, 2701, 2711, 2712, 2716, 2718, + 2719, 2722, 2724, 2727, 2728, 2730, 2732, 2735, 2736, 2739, 2740, 2756, + 2757, 2759, 2774, 2780, 2782, 2783, 2787, 2788, 2792, 2793, 2798, 2799, + 2800, 2801, 2802, 2803, 2807, 2811, 2813, 2815, 2816, 2817, 2819, 2820, + 2821, 2822, 2825, 2831, 2832, 2838, 2839, 2842, 2847, 2849, 2850, 2852, + 2855, 2856, 2860, 2866, 2871, 2873, 2874, 2876, 2877, 2895, 2901, 2904, + 2909, 2910, 2916, 2918, 2919, 2929, 2932, 2933, 2953, 2954, 2955, 2956, + 2958, 2959, 2962, 2964, 2967, 2968, 2972, 2973, 2974, 2976, 2987, 2999, + 3016, 3022, 3024, 3025, 3029, 3030, 3032, 3033, 3038, 3039, 3040, 3043, + 3044, 3045, 3047, 3052, 3053, 3059, 3060, 3061, 3063, 3068, 3071, 3072, + 3073, 3074, 3075, 3078, 3079, 3082, 3087, 3090, 3092, 3093, 3094, 3095, + 3096, 3097, 3100, 3107, 3112, 3116, 3117, 3137, 3143, 3145, 3146, 3147, + 3148, 3149, 3152, 3158, 3160, 3161, 3162, 3164, 3165, 3166, 3167, 3172, + 3175, 3176, 3177, 3180, 3181, 3182, 3183, 3186, 3188, 3192, 3193, 3194, + 3198, 3210, 3212, 3213, 3214, 3215, 3217, 3222, 3226, 3231, 3232, 3233, + 3234, 3236, 3255, 3269, 3270, 3276, 3279, 3289, 3309, 3310, 3314, 3315, + 3316, 3319, 3324, 3328, 3331, 3334, 3336, 3347, 3350, 3359, 3366, 3390, + 3395, 3409, 3429, 3435, 3436, 3443, 3467, 3470, 3478, 3479, 3504, 3509, + 3510, 3518, 3519, 3532, 3533, 3549, 3550, 3553, 3554, 3555, 3558, 3559, + 3562, 3567, 3571, 3572, 3573, 3574, 3576, 3587, 3590, 3637, 3648, 3652, + 3670, 3673, 3677, 3681, 3685, 3691, 3693, 3698, 3749, 3757, 3758, 3770, + 3772, 3789, 3790, 3793, 3794, 3797, 3798, 3800, 3806, 3811, 3812, 3813, + 3814, 3818, 3830, 3888, 3890, 3893, 3920, 3921, 3922, 3925, 3926, 3927, + 3989, 3990, 3999, 4024, 4029, 4030, 4034, 4035, 4039, 4051, 4054, 4056, + 4063, 4067, 4070, 4109, 4118, 4132, 4144, 4149, 4150, 4153, 4154, 4158, + 4171, 4172, 4173, 4174, 4183, 4190, 4237, 4252, 4264, 4270, 4273, 4277, + 4291, 4293, 4298, 4303, 4325, 4354, 4361, 4363, 4369, 4371, 4374, 4382, + 4385, 4391, 4396, 4409, 4419, 4420, 4421, 4429, 4431, 4439, 4442, 4474, + 4475, 4491, 4494, 4505, 4523, 4529, 4539, 4549, 4558, 4590, 4609, 4624, + 4629, 4635, 4636, 4663, 4667, 4670, 4679, 4708, 4713, 4731, 4737, 4739, + 4745, 4769, 4785, 4788, 4789, 4794, 4797, 4827, 4828, 4832, 4855, 4857, + 4861, 4905, 4908, 4909, 4913, 4914, 4916, 4950, 4984, 4989, 4995, 5023, + 5027, 5030, 5067, 5071, 5095, 5098, 5145, 5148, 5153, 5155, 5189, 5224, + 5229, 5230, 5234, 5251, 5254, 5263, 5270, 5308, 5337, 5385, 5388, 5389, + 5394, 5427, 5455, 5505, 5508, 5513, 5572, 5584, 5590, 5593, 5611, 5613, + 5623, 5680, 5684, 5692, 5704, 5707, 5708, 5710, 5712, 5713, 5731, 5733, + 5735, 5737, 5743, 5744, 5790, 5803, 5805, 5823, 5824, 5827, 5829, 5831, + 5835, 5860, 5863, 5864, 5867, 5870, 5872, 5921, 5925, 5926, 5942, 5943, + 5946, 5981, 5982, 5985, 5989, 5991, 5992, 6041, 6062, 6101, 6105, 6109, + 6111, 6184, 6190, 6211, 6223, 6281, 6285, 6286, 6302, 6303, 6306, 6307, + 6309, 6341, 6342, 6344, 6349, 6350, 6351, 6352, 6424, 6429, 6463, 6470, + 6585, 6589, 6644, 6664, 6667, 6668, 6670, 6691, 6697, 6703, 6704, 6825, + 6828, 6904, 6907, 6943, 6944, 7006, 7024, 7026, 7027, 7062, 7063, 7064, + 7088, 7110, 7121, 7123, 7125, 7126, 7131, 7142, 7143, 7145, 7146, 7151, + 7155, 7169, 7180, 7181, 7182, 7187, 7189, 7191, 7192, 7208, 7230, 7241, + 7243, 7245, 7246, 7251, 7262, 7263, 7265, 7266, 7267, 7269, 7271, 7275, + 7289, 7300, 7302, 7304, 7307, 7310, 7311, 7312, 7362, 7376, 7421, 7425, + 7426, 7428, 7504, 7543, 7665, 7726, 7746, 7747, 7781, 7782, 7784, 7785, + 7846, 7864, 7866, 7867, 7901, 7902, 7903, 7904, 7966, 7986, 8021, 8022, + 8025, 8141, 8145, 8201, 8203, 8211, 8222, 8225, 8231, 8249, 8260, 8261, + 8265, 8269, 8271, 8317, 8328, 8332, 8353, 8357, 8361, 8365, 8373, 8378, + 8420, 8427, 8428, 8431, 8432, 8433, 8450, 8451, 8453, 8455, 8457, 8458, + 8459, 8461, 8465, 8480, 8482, 8486, 8487, 8489, 8513, 8514, 8515, 8516, + 8517, 8565, 8583, 8584, 8587, 8589, 8623, 8624, 8630, 8632, 8681, 8685, + 8686, 8702, 8703, 8704, 8706, 8707, 8709, 8742, 8743, 8744, 8750, 8751, + 8752, 8808, 8810, 8840, 8841, 8845, 8846, 8905, 8909, 8912, 8918, 8920, + 8924, 8925, 8927, 8932, 8940, 8941, 8943, 8947, 8948, 8949, 8950, 8952, + 8953, 8954, 8958, 8970, 8971, 8972, 8973, 8974, 8975, 8977, 8984, 8990, + 8992, 8996, 9021, 9036, 9037, 9038, 9039, 9049, 9050, 9053, 9076, 9077, + 9078, 9079, 9080, 9082, 9084, 9086, 9087, 9088, 9092, 9096, 9098, 9119, + 9168, 9201, 9205, 9274, 9291, 9294, 9305, 9329, 9339, 9345, 9349, 9387, + 9391, 9397, 9400, 9402, 9415, 9416, 9418, 9432, 9437, 9455, 9458, 9461, + 9466, 9468, 9473, 9475, 9522, 9524, 9526, 9536, 9546, 9548, 9577, 9581, + 9582, 9585, 9586, 9588, 9614, 9628, 9633, 9639, 9641, 9642, 9643, 9647, + 9651, 9656, 9657, 9659, 9660, 9662, 9665, 9671, 9679, 9689, 9690, 9696, + 9700, 9701, 9706, 9708, 9709, 9711, 9714, 9717, 9751, 9752, 9757, 9758, + 9760, 9767, 9768, 9770, 9778, 9780, 9781, 9792, 9797, 9798, 9800, 9801, + 9805, 9806, 9810, 9812, 9815, 9818, 9835, 9836, 9869, 9884, 9885, 9887, + 9900, 9903, 9904, 9907, 9908, 9909, 9910, 9914, 9930, 9931, 9934, 9937, + 9943, 9944, 9950, 9952, 9986, 9987, 9991, 9997, 10000, 10002, 10004, 10006, + 10012, 10015, 10016, 10018, 10026, 10028, 10032, 10033, 10037, 10053, 10055, + 10057, 10058, 10062, 10066, 10073, 10075, 10096, 10109, 10110, 10113, 10119, + 10123, 10124, 10125, 10127, 10139, 10140, 10143, 10147, 10148, 10149, 10150, + 10151, 10154, 10155, 10159, 10170, 10171, 10174, 10176, 10177, 10180, 10184, + 10187, 10190, 10192, 10197, 10225, 10229, 10231, 10232, 10237, 10238, 10240, + 10244, 10245, 10247, 10250, 10252, 10258, 10260, 10261, 10263, 10268, 10272, + 10273, 10274, 10277, 10278, 10280, 10286, 10290, 10292, 10293, 10294, 10295, + 10297, 10298, 10312, 10315, 10316, 10351, 10357, 10360, 10364, 10368, 10372, + 10378, 10388, 10392, 10393, 10397, 10401, 10405, 10413, 10415, 10417, 10418, + 10435, 10462, 10471, 10472, 10473, 10477, 10478, 10479, 10480, 10483, 10487, + 10490, 10493, 10498, 10499, 10500, 10501, 10511, 10512, 10517, 10518, 10519, + 10520, 10522, 10526, 10527, 10530, 10532, 10535, 10536, 10538, 10540, 10555, + 10556, 10557, 10587, 10591, 10597, 10600, 10602, 10608, 10615, 10616, 10618, + 10632, 10637, 10641, 10645, 10655, 10658, 10666, 10673, 10675, 10711, 10717, + 10720, 10724, 10732, 10738, 10747, 10748, 10750, 10752, 10753, 10757, 10771, + 10773, 10775, 10777, 10778, 10784, 10795, 10827, 10840, 10842, 10855, 10856, + 10872, 10895, 10901, 10905, 10906, 10908, 10913, 10943, 10947, 10948, 10951, + 10952, 10957, 10958, 10960, 10961, 10962, 10967, 10970, 10975, 10976, 10977, + 10978, 10980, 10981, 10982, 10992, 10997, 10998, 11000, 11006, 11010, 11012, + 11015, 11018, 11026, 11031, 11033, 11034, 11035, 11036, 11057, 11068, 11069, + 11081, 11082, 11084, 11085, 11086, 11087, 11096, 11097, 11100, 11102, 11103, + 11106, 11108, 11114, 11130, 11134, 11137, 11141, 11142, 11146, 11148, 11149, + 11151, 11152, 11154, 11177, 11188, 11189, 11201, 11202, 11204, 11205, 11206, + 11207, 11216, 11217, 11220, 11222, 11223, 11226, 11227, 11228, 11229, 11230, + 11234, 11250, 11251, 11254, 11257, 11262, 11264, 11266, 11270, 11271, 11272, + 11274, 11311, 11317, 11320, 11328, 11338, 11352, 11357, 11361, 11365, 11375, + 11378, 11395, 11426, 11427, 11440, 11442, 11444, 11446, 11452, 11455, 11456, + 11466, 11468, 11472, 11473, 11493, 11495, 11497, 11501, 11502, 11506, 11508, + 11513, 11543, 11547, 11548, 11552, 11558, 11560, 11561, 11562, 11567, 11575, + 11576, 11577, 11580, 11581, 11582, 11592, 11598, 11610, 11612, 11615, 11621, + 11626, 11628, 11629, 11631, 11633, 11634, 11636, 11682, 11684, 11686, 11696, + 11706, 11707, 11708, 11710, 11731, 11737, 11741, 11742, 11744, 11746, 11748, + 11788, 11801, 11802, 11807, 11816, 11817, 11820, 11822, 11850, 11861, 11865, + 11866, 11868, 11869, 11871, 11874, 11922, 11924, 11926, 11936, 11944, 11946, + 11947, 11948, 11950, 11971, 11977, 11982, 11983, 11984, 11986, 12051, 12065, + 12089, 12105, 12109, 12157, 12158, 12159, 12168, 12170, 12173, 12197, 12198, + 12199, 12200, 12201, 12202, 12205, 12206, 12207, 12212, 12216, 12218, 12277, + 12278, 12288, 12290, 12317, 12318, 12320, 12321, 12325, 12326, 12332, 12338, + 12397, 12408, 12437, 12441, 12445, 12458, 12491, 12508, 12513, 12514, 12516, + 12531, 12534, 12537, 12539, 12545, 12564, 12568, 12569, 12579, 12588, 12589, + 12594, 12597, 12620, 12627, 12628, 12632, 12633, 12651, 12653, 12655, 12657, + 12659, 12661, 12665, 12682, 12687, 12689, 12708, 12709, 12713, 12714, 12716, + 12717, 12747, 12748, 12751, 12752, 12770, 12775, 12777, 12778, 12781, 12800, + 12806, 12828, 12829, 12833, 12834, 12835, 12836, 12867, 12871, 12888, 12895, + 12898, 12921, 12925, 12948, 12953, 12955, 12996, 13008, 13010, 13013, 13040, + 13041, 13042, 13044, 13045, 13046, 13047, 13048, 13106, 13107, 13120, 13122, + 13124, 13126, 13132, 13135, 13136, 13146, 13147, 13148, 13150, 13152, 13153, + 13171, 13173, 13175, 13177, 13182, 13184, 13186, 13193, 13207, 13230, 13234, + 13243, 13245, 13249, 13254, 13263, 13267, 13269, 13271, 13275, 13276, 13299, + 13300, 13304, 13307, 13310, 13312, 13319, 13338, 13355, 13356, 13370, 13373, + 13400, 13402, 13403, 13404, 13406, 13407, 13408, 13438, 13459, 13471, 13472, + 13473, 13474, 13476, 13490, 13493, 13494, 13498, 13499, 13501, 13520, 13522, + 13524, 13526, 13527, 13528, 13539, 13555, 13556, 13557, 13591, 13592, 13593, + 13608, 13610, 13613, 13618, 13619, 13621, 13640, 13641, 13642, 13645, 13646, + 13647, 13675, 13676, 13677, 13711, 13712, 13728, 13730, 13738, 13741, 13760, + 13761, 13765, 13766, 13795, 13796, 13831, 13848, 13858, 13881, 13885, 13915, + 13944, 13949, 13950, 13957, 13958, 13959, 13970, 13972, 13973, 13993, 13994, + 13995, 13997, 13998, 13999, 14000, 14002, 14006, 14007, 14012, 14013, 14014, + 14016, 14018, 14027, 14069, 14077, 14078, 14088, 14090, 14092, 14113, 14114, + 14117, 14118, 14120, 14121, 14125, 14126, 14132, 14133, 14134, 14138, 14187, + 14188, 14191, 14192, 14208, 14210, 14215, 14217, 14218, 14221, 14240, 14241, + 14245, 14246, 14273, 14274, 14275, 14276, 14307, 14311, 14328, 14335, 14338, + 14361, 14365, 14393, 14395 + }; + + const double values[1968] = { + u[0], a[0], b[0], c[0], u[3], u[0], a[0], a[9], b[0], b[9], c[0], c[9], + u[3], u[0], a[9], a[13], a[0], b[9], b[0], b[13], c[0], c[9], c[13], u[2], + u[0], a[10], a[0], b[0], b[10], c[10], c[0], u[2], u[3], u[0], a[10], a[4], + a[0], a[9], b[10], b[0], b[9], b[4], c[10], c[0], c[4], c[9], u[2], u[3], + u[0], a[4], a[11], a[0], a[9], a[13], a[10], b[4], b[0], b[10], b[9], b[13], + b[11], c[10], c[4], c[9], c[0], c[11], c[13], u[2], u[0], a[0], a[14], + a[10], b[0], b[10], b[14], c[0], c[14], c[10], u[2], u[3], u[0], a[9], + a[14], a[5], a[10], a[0], a[4], b[14], b[0], b[10], b[9], b[4], b[5], c[14], + c[10], c[9], c[0], c[5], c[4], u[2], u[3], u[0], a[13], a[5], a[10], a[4], + a[9], a[0], a[11], a[14], b[5], b[10], b[9], b[14], b[0], b[4], b[13], + b[11], c[14], c[5], c[4], c[0], c[13], c[10], c[9], c[11], u[1], u[0], a[8], + a[0], b[8], b[0], c[0], c[8], u[1], u[3], u[0], a[0], a[8], a[3], a[9], + b[8], b[0], b[3], b[9], c[9], c[8], c[0], c[3], u[1], u[3], u[0], a[0], + a[9], a[3], a[7], a[13], a[8], b[3], b[0], b[9], b[8], b[7], b[13], c[13], + c[8], c[3], c[0], c[9], c[7], u[1], u[2], u[0], a[2], a[10], a[0], a[8], + b[8], b[0], b[2], b[10], c[10], c[0], c[2], c[8], u[1], u[2], u[3], u[0], + a[10], a[2], a[16], a[4], a[9], a[8], a[0], a[3], b[2], b[10], b[0], b[8], + b[3], b[9], b[16], b[4], c[4], c[0], c[9], c[2], c[8], c[10], c[16], c[3], + u[1], u[2], u[3], u[0], a[10], a[4], a[16], a[11], a[13], a[8], a[0], a[3], + a[9], a[7], a[2], b[16], b[0], b[10], b[4], b[9], b[8], b[2], b[3], b[7], + b[13], b[11], c[11], c[2], c[9], c[13], c[16], c[3], c[0], c[8], c[10], + c[4], c[7], u[1], u[2], u[0], a[0], a[8], a[17], a[14], a[10], a[2], b[0], + b[8], b[2], b[10], b[17], b[14], c[14], c[0], c[10], c[8], c[17], c[2], + u[1], u[2], u[3], u[0], a[9], a[3], a[14], a[17], a[5], a[4], a[2], a[0], + a[8], a[10], a[16], b[17], b[14], b[10], b[8], b[0], b[2], b[9], b[3], + b[16], b[4], b[5], c[5], c[10], c[9], c[4], c[0], c[17], c[2], c[3], c[8], + c[14], c[16], u[1], u[2], u[3], u[0], a[14], a[13], a[7], a[5], a[11], a[2], + a[10], a[16], a[9], a[3], a[8], a[4], a[17], b[10], b[14], b[5], b[4], b[2], + b[3], b[17], b[8], b[9], b[16], b[13], b[7], b[11], c[17], c[4], c[13], + c[11], c[9], c[16], c[8], c[10], c[7], c[2], c[3], c[14], c[5], u[1], u[0], + a[12], a[8], a[0], b[0], b[12], b[8], c[0], c[8], c[12], u[1], u[3], u[0], + a[0], a[8], a[12], a[18], a[3], a[9], b[12], b[8], b[0], b[9], b[18], b[3], + c[9], c[3], c[12], c[0], c[8], c[18], u[1], u[3], u[0], a[0], a[8], a[9], + a[3], a[18], a[7], a[12], a[13], b[18], b[0], b[8], b[3], b[9], b[12], + b[13], b[7], c[13], c[7], c[12], c[18], c[0], c[8], c[9], c[3], u[1], u[2], + u[0], a[1], a[2], a[0], a[8], a[12], a[10], b[12], b[0], b[8], b[10], b[1], + b[2], c[10], c[2], c[0], c[8], c[1], c[12], u[1], u[2], u[3], u[0], a[10], + a[2], a[1], a[16], a[9], a[3], a[0], a[12], a[8], a[18], a[4], b[1], b[2], + b[8], b[10], b[12], b[0], b[18], b[9], b[3], b[4], b[16], c[4], c[16], c[8], + c[9], c[0], c[3], c[1], c[12], c[10], c[2], c[18], u[1], u[2], u[3], u[0], + a[10], a[2], a[4], a[16], a[13], a[7], a[9], a[12], a[8], a[18], a[3], a[1], + a[11], b[10], b[8], b[2], b[16], b[3], b[4], b[12], b[1], b[18], b[9], + b[13], b[7], b[11], c[11], c[1], c[3], c[13], c[9], c[7], c[18], c[8], + c[12], c[10], c[2], c[4], c[16], u[1], u[2], u[0], a[8], a[12], a[17], + a[10], a[2], a[1], a[0], a[14], b[0], b[8], b[12], b[1], b[10], b[2], b[14], + b[17], c[14], c[17], c[10], c[0], c[8], c[2], c[12], c[1], u[1], u[2], u[3], + u[0], a[3], a[18], a[14], a[17], a[4], a[16], a[10], a[1], a[8], a[12], + a[2], a[9], a[5], b[17], b[2], b[14], b[12], b[8], b[1], b[10], b[9], b[3], + b[18], b[4], b[16], b[5], c[5], c[2], c[4], c[9], c[3], c[10], c[16], c[8], + c[1], c[18], c[12], c[14], c[17], u[1], u[2], u[3], u[0], a[14], a[17], + a[7], a[5], a[11], a[4], a[1], a[2], a[3], a[18], a[12], a[16], a[13], + b[14], b[2], b[17], b[16], b[5], b[1], b[18], b[12], b[3], b[4], b[13], + b[7], b[11], c[16], c[11], c[13], c[7], c[4], c[3], c[12], c[2], c[1], + c[18], c[14], c[17], c[5], u[2], a[10], a[2], a[6], a[14], a[17], b[8], + b[0], b[10], b[2], b[17], b[14], b[6], c[6], c[0], c[10], c[14], c[2], c[8], + c[17], u[2], a[10], a[6], a[14], b[0], b[10], b[14], b[6], c[10], c[0], + c[6], c[14], u[2], a[2], a[1], a[14], a[17], a[10], a[6], b[12], b[8], + b[10], b[2], b[1], b[14], b[17], b[6], c[6], c[8], c[14], c[10], c[2], + c[17], c[1], c[12], a[17], a[6], a[1], b[19], b[1], b[17], b[6], c[6], + c[19], c[1], c[17], a[1], a[14], a[17], a[6], a[2], b[19], b[12], b[2], + b[1], b[14], b[17], b[6], c[6], c[12], c[17], c[2], c[1], c[14], c[19], + a[8], a[12], a[19], b[12], b[8], b[19], c[8], c[12], c[19], a[14], a[17], + a[6], b[8], b[2], b[10], b[14], b[17], b[6], c[10], c[14], c[6], c[8], + c[17], c[2], a[17], a[6], a[14], b[12], b[1], b[2], b[14], b[17], b[6], + c[2], c[6], c[14], c[17], c[12], c[1], a[6], a[17], b[19], b[1], b[17], + b[6], c[1], c[17], c[6], c[19], u[3], a[11], a[9], a[13], a[15], a[4], + b[11], b[9], b[4], b[13], b[15], c[4], c[11], c[13], c[0], c[10], c[9], + c[15], u[3], a[13], a[15], a[9], b[13], b[9], b[15], c[9], c[13], c[0], + c[15], a[14], a[6], b[0], b[10], b[14], b[6], c[0], c[14], c[10], c[6], + a[13], a[15], a[7], b[13], b[15], b[7], c[7], c[8], c[9], c[3], c[13], + c[15], a[13], a[7], a[15], b[13], b[7], b[15], c[12], c[3], c[18], c[13], + c[7], c[15], a[6], b[10], b[14], b[6], c[10], c[6], c[14], a[7], a[15], + b[7], b[15], c[19], c[18], c[7], c[15], a[6], b[2], b[17], b[14], b[6], + c[14], c[6], c[2], c[17], a[15], b[15], c[3], c[13], c[7], c[15], a[15], + b[15], c[18], c[7], c[15], a[6], b[1], b[17], b[6], c[17], c[6], c[1], a[6], + a[17], a[5], b[18], b[1], b[17], b[16], b[6], b[5], c[16], c[5], c[6], + c[17], c[18], c[1], a[5], a[6], a[14], b[14], b[9], b[10], b[4], b[6], b[5], + c[6], c[9], c[10], c[5], c[4], c[14], a[11], a[15], a[13], b[11], b[15], + b[13], c[4], c[13], c[14], c[5], c[11], c[15], a[15], b[15], c[13], c[4], + c[11], c[15], b[17], b[6], c[6], c[17], a[5], a[11], a[4], b[5], b[11], + b[4], b[13], b[15], c[14], c[4], c[13], c[6], c[15], c[5], c[11], b[14], + b[6], c[14], c[6], c[13], c[15], a[6], b[16], b[17], b[6], b[5], c[5], c[6], + c[16], c[17], c[7], c[15], b[5], b[6], c[5], c[6], a[6], b[11], b[6], b[5], + c[6], c[11], c[5], u[3], a[15], a[4], a[11], a[13], a[9], a[5], b[4], b[13], + b[5], b[9], b[11], b[15], c[5], c[11], c[10], c[9], c[15], c[14], c[4], + c[13], u[2], a[11], a[14], a[5], a[4], a[10], a[6], b[14], b[4], b[6], + b[10], b[9], b[13], b[5], b[11], c[6], c[5], c[10], c[9], c[11], c[13], + c[14], c[4], a[15], b[15], c[7], c[16], c[15], c[11], b[6], c[6], c[15], + a[11], b[11], b[15], c[5], c[11], c[15], c[6], a[5], b[15], b[5], b[11], + c[6], c[5], c[15], c[11], a[15], b[15], c[11], c[15], c[5], c[15], c[11], + a[13], a[15], a[11], b[13], b[11], b[15], c[11], c[15], c[9], c[10], c[4], + c[13], a[1], a[17], a[19], b[19], b[1], b[17], c[17], c[19], c[1], u[1], + a[8], a[12], a[9], a[3], a[18], a[13], a[19], a[7], b[8], b[12], b[9], + b[18], b[3], b[19], b[13], b[7], c[13], c[7], c[19], c[8], c[12], c[9], + c[3], c[18], a[6], b[6], b[4], b[14], b[5], c[4], c[14], c[5], c[6], a[6], + a[5], a[14], b[6], b[5], b[13], b[14], b[4], b[11], c[14], c[13], c[4], + c[11], c[6], c[5], a[12], a[19], b[19], b[12], c[12], c[19], u[2], a[16], + a[6], a[5], a[14], a[2], a[1], a[17], a[4], b[17], b[6], b[1], b[12], b[2], + b[18], b[3], b[14], b[4], b[16], b[5], c[17], c[3], c[5], c[4], c[16], + c[14], c[2], c[12], c[18], c[1], c[6], u[1], a[1], a[0], a[8], a[12], a[19], + a[10], a[2], b[19], b[0], b[8], b[12], b[10], b[2], b[1], c[10], c[2], c[1], + c[8], c[12], c[0], c[19], a[19], b[19], c[19], a[15], a[13], b[15], b[13], + c[13], c[15], c[0], c[9], a[16], a[11], a[15], a[7], a[18], b[16], b[18], + b[11], b[7], b[15], c[7], c[15], c[19], c[18], c[1], c[16], c[11], a[11], + a[15], a[7], b[11], b[7], b[15], c[15], c[16], c[7], c[17], c[11], c[5], + u[3], a[4], a[11], a[15], a[3], a[9], a[7], a[13], a[16], b[9], b[4], b[11], + b[13], b[3], b[16], b[7], b[15], c[16], c[13], c[15], c[7], c[8], c[9], + c[10], c[2], c[3], c[4], c[11], a[2], a[1], a[3], a[18], a[12], a[19], a[4], + a[16], b[2], b[19], b[1], b[12], b[3], b[18], b[16], b[4], c[4], c[16], + c[19], c[18], c[12], c[3], c[2], c[1], a[5], a[14], a[17], a[6], b[6], + b[17], b[3], b[2], b[14], b[16], b[4], b[5], c[6], c[4], c[5], c[14], c[3], + c[2], c[16], c[17], u[1], a[17], a[5], a[11], a[16], a[1], a[18], a[19], + a[7], b[17], b[1], b[5], b[19], b[18], b[16], b[7], b[11], c[7], c[16], + c[18], c[11], c[19], c[1], c[17], c[5], u[2], a[4], a[16], a[6], a[5], + a[17], a[10], a[2], a[14], b[6], b[14], b[2], b[8], b[10], b[3], b[9], + b[17], b[4], b[16], b[5], c[14], c[9], c[4], c[5], c[10], c[17], c[8], + c[16], c[3], c[2], c[6], u[1], a[18], a[14], a[17], a[4], a[16], a[2], + a[12], a[19], a[1], a[5], a[3], b[14], b[1], b[17], b[19], b[12], b[2], + b[3], b[18], b[4], b[16], b[5], c[5], c[1], c[16], c[3], c[18], c[2], c[12], + c[4], c[19], c[14], c[17], a[17], a[16], a[1], a[19], a[5], a[18], b[17], + b[19], b[1], b[18], b[16], b[5], c[5], c[18], c[1], c[19], c[16], c[17], + u[1], a[10], a[2], a[1], a[9], a[3], a[18], a[8], a[19], a[12], a[4], a[16], + b[10], b[1], b[12], b[2], b[19], b[8], b[9], b[3], b[18], b[4], b[16], c[4], + c[16], c[12], c[3], c[8], c[18], c[9], c[19], c[10], c[2], c[1], a[1], + a[16], a[7], a[18], a[19], a[11], b[1], b[19], b[16], b[18], b[7], b[11], + c[11], c[18], c[7], c[19], c[1], c[16], a[6], a[5], a[17], a[1], a[16], + b[6], b[19], b[1], b[18], b[17], b[16], b[5], c[18], c[16], c[17], c[1], + c[5], c[19], c[6], a[11], a[15], a[7], b[11], b[7], b[15], c[15], c[18], + c[1], c[7], c[16], c[11], u[1], a[2], a[1], a[4], a[16], a[13], a[7], a[3], + a[19], a[12], a[18], a[11], b[2], b[12], b[1], b[4], b[18], b[16], b[19], + b[3], b[13], b[7], b[11], c[11], c[18], c[7], c[3], c[13], c[12], c[19], + c[2], c[1], c[4], c[16], u[3], a[5], a[15], a[16], a[4], a[13], a[7], a[3], + a[11], b[4], b[5], b[11], b[16], b[7], b[3], b[13], b[15], c[11], c[15], + c[13], c[2], c[3], c[4], c[14], c[17], c[16], c[7], c[5], u[2], a[6], a[11], + a[17], a[14], a[4], a[16], a[2], a[5], b[14], b[6], b[5], b[17], b[16], + b[2], b[3], b[4], b[7], b[13], b[11], c[5], c[13], c[11], c[4], c[2], c[3], + c[14], c[7], c[17], c[16], c[6], a[1], a[18], a[19], a[16], b[1], b[19], + b[18], b[16], c[16], c[19], c[18], c[1], u[3], a[5], a[11], a[16], a[7], + a[18], a[15], b[5], b[16], b[18], b[7], b[11], b[15], c[15], c[11], c[7], + c[1], c[18], c[16], c[17], c[5], u[3], a[4], a[16], a[11], a[15], a[13], + a[18], a[3], a[7], b[4], b[3], b[16], b[7], b[11], b[18], b[13], b[15], + c[7], c[15], c[13], c[12], c[3], c[2], c[1], c[18], c[4], c[16], c[11], + a[5], a[11], a[16], b[5], b[16], b[7], b[11], b[15], c[15], c[11], c[17], + c[16], c[7], c[5], c[6], a[11], a[7], a[13], a[15], b[13], b[11], b[15], + b[7], c[15], c[3], c[2], c[13], c[4], c[16], c[7], c[11], a[6], a[5], a[17], + b[6], b[7], b[17], b[16], b[5], b[11], c[11], c[5], c[17], c[7], c[16], + c[6], a[15], b[15], c[15], c[9], c[13], a[8], a[12], a[19], a[10], a[2], + a[1], b[8], b[12], b[19], b[2], b[10], b[1], c[10], c[2], c[1], c[12], + c[19], c[8], a[12], a[19], a[2], a[1], b[12], b[19], b[1], b[2], c[2], c[1], + c[19], c[12], a[19], a[1], b[19], b[1], c[1], c[19], u[3], a[9], a[13], + a[7], a[15], a[3], b[7], b[9], b[13], b[3], b[15], c[15], c[3], c[7], c[0], + c[8], c[9], c[13], u[3], a[9], a[3], a[13], a[7], a[18], a[15], b[9], b[3], + b[7], b[13], b[18], b[15], c[15], c[18], c[8], c[12], c[9], c[3], c[13], + c[7], a[3], a[18], a[13], a[7], a[15], b[3], b[18], b[13], b[7], b[15], + c[15], c[12], c[19], c[3], c[18], c[13], c[7], a[18], a[7], a[15], b[18], + b[7], b[15], c[15], c[19], c[18], c[7], a[19], a[0], a[8], a[12], b[8], + b[0], b[12], b[19], c[0], c[8], c[12], c[19], u[2], a[6], a[5], a[17], + a[16], a[1], a[11], b[6], b[17], b[1], b[18], b[16], b[7], b[5], b[11], + c[7], c[11], c[5], c[16], c[1], c[18], c[17], c[6], u[2], a[4], a[6], a[14], + a[10], a[5], b[6], b[10], b[0], b[9], b[14], b[4], b[5], c[6], c[14], c[0], + c[4], c[9], c[10], c[5], u[1], a[19], a[12], a[0], a[8], b[0], b[8], b[19], + b[12], c[0], c[8], c[12], c[19], u[1], a[0], a[8], a[12], a[19], a[18], + a[9], a[3], b[19], b[0], b[12], b[8], b[9], b[3], b[18], c[9], c[3], c[18], + c[19], c[0], c[8], c[12], a[8], a[12], a[19], a[9], a[3], a[18], b[8], + b[19], b[12], b[3], b[9], b[18], c[9], c[3], c[18], c[8], c[12], c[19], + a[12], a[19], a[3], a[18], b[12], b[19], b[18], b[3], c[3], c[18], c[12], + c[19], a[19], a[18], b[19], b[18], c[18], c[19], u[1], a[12], a[19], a[10], + a[2], a[1], a[14], a[8], a[17], b[8], b[12], b[19], b[10], b[2], b[1], + b[14], b[17], c[14], c[17], c[2], c[8], c[12], c[1], c[10], c[19], a[19], + a[2], a[1], a[14], a[17], a[12], b[12], b[19], b[2], b[1], b[17], b[14], + c[14], c[17], c[1], c[12], c[19], c[2], a[12], a[19], a[3], a[18], a[13], + a[7], b[12], b[19], b[3], b[18], b[7], b[13], c[13], c[7], c[12], c[19], + c[3], c[18], a[19], a[18], a[7], b[19], b[18], b[7], c[7], c[19], c[18] + }; + + Eigen::Map > macaulay_vec(macaulay_matrix.data()); + for (int i = 0; i < 1968; i++) { + macaulay_vec(indices[i]) = values[i]; + } + } + + } + } +} \ No newline at end of file diff --git a/include/solver_p3p.h b/include/solver_p3p.h new file mode 100644 index 0000000..d115add --- /dev/null +++ b/include/solver_p3p.h @@ -0,0 +1,368 @@ +// Copyright (C) 2019 Czech Technical University. +// 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 Czech Technical University 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 HOLDERS 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. +// +// Please contact the author of this library if you have any questions. +// Author: Daniel Barath (barath.daniel@sztaki.mta.hu) +#pragma once + +#include "solver_engine.h" +#include "fundamental_estimator.h" +#include + +namespace gcransac +{ + namespace estimator + { + namespace solver + { + // This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented + class P3PSolver : public SolverEngine + { + public: + P3PSolver() + { + } + + ~P3PSolver() + { + } + + // Determines if there is a chance of returning multiple models + // the function 'estimateModel' is applied. + static constexpr bool returnMultipleModels() + { + return true; + } + + // The minimum number of points required for the estimation + static constexpr size_t sampleSize() + { + return 3; + } + + // Estimate the model parameters from the given point sample + // using weighted fitting if possible. + inline bool estimateModel( + const cv::Mat& data_, // The set of data points + const size_t *sample_, // The sample used for the estimation + size_t sample_number_, // The size of the sample + std::vector &models_, // The estimated model parameters + const double *weights_ = nullptr) const; // The weight for each point + + inline size_t solvePlaneRotation( + const Eigen::Vector3d normalized_image_points[3], + const Eigen::Vector3d& intermediate_image_point, + const Eigen::Vector3d& intermediate_world_point, + const double d_12, + double cos_theta[4], + double cot_alphas[4], + double* b) const; + + inline void backsubstitute( + const Eigen::Matrix3d& intermediate_world_frame, + const Eigen::Matrix3d& intermediate_camera_frame, + const Eigen::Vector3d& world_point_0, + const double cos_theta, + const double cot_alpha, + const double d_12, + const double b, + Eigen::Vector3d& translation, + Eigen::Matrix3d& rotation) const; + }; + + // Solves for cos(theta) that will describe the rotation of the plane from + // intermediate world frame to intermediate camera frame. The method returns the + // roots of a quartic (i.e. solutions to cos(alpha) ) and several factors that + // are needed for back-substitution. + size_t P3PSolver::solvePlaneRotation( + const Eigen::Vector3d normalized_image_points[3], + const Eigen::Vector3d& intermediate_image_point, + const Eigen::Vector3d& intermediate_world_point, + const double d_12, + double cos_theta[4], + double cot_alphas[4], + double* b) const + { + // Calculate these parameters ahead of time for reuse and + // readability. Notation for these variables is consistent with the notation + // from the paper. + const double f_1 = + intermediate_image_point[0] / intermediate_image_point[2]; + const double f_2 = + intermediate_image_point[1] / intermediate_image_point[2]; + const double p_1 = intermediate_world_point[0]; + const double p_2 = intermediate_world_point[1]; + const double cos_beta = + normalized_image_points[0].dot(normalized_image_points[1]); + *b = 1.0 / (1.0 - cos_beta * cos_beta) - 1.0; + + if (cos_beta < 0) + *b = -sqrt(*b); + else + *b = sqrt(*b); + + // Definition of temporary variables for readability in the coefficients + // calculation. + const double f_1_pw2 = f_1 * f_1; + const double f_2_pw2 = f_2 * f_2; + const double p_1_pw2 = p_1 * p_1; + const double p_1_pw3 = p_1_pw2 * p_1; + const double p_1_pw4 = p_1_pw3 * p_1; + const double p_2_pw2 = p_2 * p_2; + const double p_2_pw3 = p_2_pw2 * p_2; + const double p_2_pw4 = p_2_pw3 * p_2; + const double d_12_pw2 = d_12 * d_12; + const double b_pw2 = (*b) * (*b); + + // Computation of coefficients of 4th degree polynomial. + Eigen::VectorXd coefficients(5); + coefficients(4) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4; + coefficients(3) = + 2.0 * p_2_pw3 * d_12 * (*b) + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) - + 2.0 * f_2 * p_2_pw3 * f_1 * d_12; + coefficients(2) = + -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - + f_2_pw2 * p_2_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + + 2.0 * p_1 * p_2_pw2 * d_12 + + 2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * (*b) - + p_2_pw2 * p_1_pw2 * f_1_pw2 + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - + p_2_pw2 * d_12_pw2 * b_pw2 - 2.0 * p_1_pw2 * p_2_pw2; + coefficients(1) = + 2.0 * p_1_pw2 * p_2 * d_12 * (*b) + 2.0 * f_2 * p_2_pw3 * f_1 * d_12 - + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) - 2.0 * p_1 * p_2 * d_12_pw2 * (*b); + coefficients(0) = + -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * (*b) + + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2.0 * p_1_pw3 * d_12 - p_1_pw2 * d_12_pw2 + + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 - + 2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 + p_2_pw2 * f_1_pw2 * p_1_pw2 + + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2; + + // Computation of roots. + Eigen::PolynomialSolver psolve(coefficients); + std::vector roots; + psolve.realRoots(roots); + + // Calculate cot(alpha) needed for back-substitution. + for (size_t i = 0; i < roots.size(); i++) { + cos_theta[i] = std::clamp(roots[i], -1.0, 1.0); + cot_alphas[i] = (-f_1 * p_1 / f_2 - cos_theta[i] * p_2 + d_12 * (*b)) / + (-f_1 * cos_theta[i] * p_2 / f_2 + p_1 - d_12); + } + + return roots.size(); + } + + // Given the complete transformation between intermediate world and camera + // frames (parameterized by cos_theta and cot_alpha), back-substitute the + // solution and get an absolute camera pose. + void P3PSolver::backsubstitute( + const Eigen::Matrix3d& intermediate_world_frame, + const Eigen::Matrix3d& intermediate_camera_frame, + const Eigen::Vector3d& world_point_0, + const double cos_theta, + const double cot_alpha, + const double d_12, + const double b, + Eigen::Vector3d& translation, + Eigen::Matrix3d& rotation) const + { + const double sin_theta = sqrt(1.0 - cos_theta * cos_theta); + const double sin_alpha = sqrt(1.0 / (cot_alpha * cot_alpha + 1.0)); + double cos_alpha = sqrt(1.0 - sin_alpha * sin_alpha); + + if (cot_alpha < 0) { + cos_alpha = -cos_alpha; + } + + // Get the camera position in the intermediate world frame + // coordinates. (Eq. 5 from the paper). + const Eigen::Vector3d c_nu( + d_12 * cos_alpha * (sin_alpha * b + cos_alpha), + cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha), + sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha)); + + // Transform c_nu into world coordinates. Use a Map to put the solution + // directly into the output. + translation = world_point_0 + intermediate_world_frame.transpose() * c_nu; + + // Construct the transformation from the intermediate world frame to the + // intermediate camera frame. + Eigen::Matrix3d intermediate_world_to_camera_rotation; + intermediate_world_to_camera_rotation << + -cos_alpha, -sin_alpha * cos_theta, -sin_alpha * sin_theta, + sin_alpha, -cos_alpha * cos_theta, -cos_alpha * sin_theta, + 0, -sin_theta, cos_theta; + + // Construct the rotation matrix. + rotation = (intermediate_world_frame.transpose() * + intermediate_world_to_camera_rotation.transpose() * + intermediate_camera_frame).transpose(); + + // Adjust translation to account for rotation. + translation = -rotation * translation; + } + + inline bool P3PSolver::estimateModel( + const cv::Mat& data_, + const size_t *sample_, + size_t sample_number_, + std::vector &models_, + const double *weights_) const + { + if (sample_ == nullptr) + sample_number_ = data_.rows; + + const double * data_ptr = reinterpret_cast(data_.data); + const size_t columns = data_.cols; + + Eigen::Vector3d normalized_image_points[3]; + // Store points_3d in world_points for ease of use. NOTE: we cannot use a + // const ref or a Map because the world_points entries may be swapped later. + Eigen::Vector3d world_points[3]; + for (int i = 0; i < 3; ++i) { + const size_t idx = columns * (sample_ == nullptr ? + i : sample_[i]); + + normalized_image_points[i] << + data_ptr[idx], data_ptr[idx + 1], 1.0; + normalized_image_points[i].normalize(); + + world_points[i] << + data_ptr[idx + 2], data_ptr[idx + 3], data_ptr[idx + 4]; + } + + // If the points are collinear, there are no possible solutions. + constexpr double tolerance = 1e-6; + Eigen::Vector3d world_1_0 = world_points[1] - world_points[0]; + Eigen::Vector3d world_2_0 = world_points[2] - world_points[0]; + if (world_1_0.cross(world_2_0).squaredNorm() < tolerance) { + return false; + } + + // Create intermediate camera frame such that the x axis is in the direction + // of one of the normalized image points, and the origin is the same as the + // absolute camera frame. This is a rotation defined as the transformation: + // T = [tx, ty, tz] where tx = f0, tz = (f0 x f1) / ||f0 x f1||, and + // ty = tx x tz and f0, f1, f2 are the normalized image points. + Eigen::Matrix3d intermediate_camera_frame; + intermediate_camera_frame.row(0) = normalized_image_points[0]; + intermediate_camera_frame.row(2) = + normalized_image_points[0].cross(normalized_image_points[1]).normalized(); + intermediate_camera_frame.row(1) = + intermediate_camera_frame.row(2).cross(intermediate_camera_frame.row(0)); + + // Project the third world point into the intermediate camera frame. + Eigen::Vector3d intermediate_image_point = + intermediate_camera_frame * normalized_image_points[2]; + + // Enforce that the intermediate_image_point is in front of the intermediate + // camera frame. If the point is behind the camera frame, recalculate the + // intermediate camera frame by swapping which feature we align the x axis to. + if (intermediate_image_point[2] > 0) { + std::swap(normalized_image_points[0], normalized_image_points[1]); + + intermediate_camera_frame.row(0) = normalized_image_points[0]; + intermediate_camera_frame.row(2) = normalized_image_points[0] + .cross(normalized_image_points[1]).normalized(); + intermediate_camera_frame.row(1) = intermediate_camera_frame.row(2) + .cross(intermediate_camera_frame.row(0)); + + intermediate_image_point = + intermediate_camera_frame * normalized_image_points[2]; + + std::swap(world_points[0], world_points[1]); + world_1_0 = world_points[1] - world_points[0]; + world_2_0 = world_points[2] - world_points[0]; + } + + if (abs(intermediate_image_point[2]) < std::numeric_limits::epsilon()) + return false; + + // Create the intermediate world frame transformation that has the + // origin at world_points[0] and the x-axis in the direction of + // world_points[1]. This is defined by the transformation: N = [nx, ny, nz] + // where nx = (p1 - p0) / ||p1 - p0|| + // nz = nx x (p2 - p0) / || nx x (p2 -p0) || and ny = nz x nx + // Where p0, p1, p2 are the world points. + Eigen::Matrix3d intermediate_world_frame; + intermediate_world_frame.row(0) = world_1_0.normalized(); + intermediate_world_frame.row(2) = + intermediate_world_frame.row(0).cross(world_2_0).normalized(); + intermediate_world_frame.row(1) = + intermediate_world_frame.row(2).cross(intermediate_world_frame.row(0)); + + // Transform world_point[2] to the intermediate world frame coordinates. + Eigen::Vector3d intermediate_world_point = intermediate_world_frame * world_2_0; + + // Distance from world_points[1] to the intermediate world frame origin. + double d_12 = world_1_0.norm(); + + // Solve for the cos(theta) that will give us the transformation from + // intermediate world frame to intermediate camera frame. We also get the + // cot(alpha) for each solution necessary for back-substitution. + double cos_theta[4]; + double cot_alphas[4]; + double b; + const size_t num_solutions = solvePlaneRotation( + normalized_image_points, + intermediate_image_point, + intermediate_world_point, + d_12, + cos_theta, + cot_alphas, + &b); + + // Backsubstitution of each solution + std::vector solution_translations(num_solutions); + std::vector solution_rotations(num_solutions); + models_.resize(num_solutions); + + for (size_t i = 0; i < num_solutions; i++) { + backsubstitute( + intermediate_world_frame, + intermediate_camera_frame, + world_points[0], + cos_theta[i], + cot_alphas[i], + d_12, + b, + solution_translations.at(i), + solution_rotations.at(i)); + + models_[i].descriptor = Eigen::Matrix(); + models_[i].descriptor << solution_rotations[i], solution_translations[i]; + } + + return num_solutions > 0; + } + } + } +} \ No newline at end of file diff --git a/include/types.h b/include/types.h index 86098e9..83543e7 100644 --- a/include/types.h +++ b/include/types.h @@ -39,9 +39,12 @@ #include "fundamental_estimator.h" #include "homography_estimator.h" #include "essential_estimator.h" +#include "perspective_n_point_estimator.h" #include "solver_fundamental_matrix_seven_point.h" #include "solver_fundamental_matrix_eight_point.h" +#include "solver_p3p.h" +#include "solver_dls_pnp.h" #include "solver_homography_four_point.h" #include "solver_essential_matrix_five_point_stewenius.h" @@ -64,6 +67,11 @@ namespace gcransac estimator::solver::EssentialMatrixFivePointSteweniusSolver> // The solver used for fitting a model to a non-minimal sample DefaultEssentialMatrixEstimator; + // The default estimator for PnP fitting + typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample + DefaultPnPEstimator; + struct Settings { bool do_final_iterated_least_squares, // Flag to decide a final iterated least-squares fitting is needed to polish the output model parameters. do_local_optimization, // Flag to decide if local optimization is needed @@ -92,9 +100,9 @@ namespace gcransac do_graph_cut(true), use_inlier_limit(false), desired_fps(-1), - max_local_optimization_number(std::numeric_limits::max()), - max_graph_cut_number(std::numeric_limits::max()), - max_least_squares_iterations(20), + max_local_optimization_number(10), + max_graph_cut_number(10), + max_least_squares_iterations(10), min_iteration_number_before_lo(20), min_iteration_number(20), neighborhood_sphere_radius(20),