1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4
5 // Copyright (C) 2019 Czech Technical University.
6 // All rights reserved.
7 //
8 // Redistribution and use in source and binary forms, with or without
9 // modification, are permitted provided that the following conditions are
10 // met:
11 //
12 // * Redistributions of source code must retain the above copyright
13 // notice, this list of conditions and the following disclaimer.
14 //
15 // * Redistributions in binary form must reproduce the above
16 // copyright notice, this list of conditions and the following
17 // disclaimer in the documentation and/or other materials provided
18 // with the distribution.
19 //
20 // * Neither the name of Czech Technical University nor the
21 // names of its contributors may be used to endorse or promote products
22 // derived from this software without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
25 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
26 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
27 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
28 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
29 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
30 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
31 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
32 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
33 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 // POSSIBILITY OF SUCH DAMAGE.
35 //
36 // Please contact the author of this library if you have any questions.
37 // Author: Daniel Barath (barath.daniel@sztaki.mta.hu)
38 // Modification: Maksym Ivashechkin (ivashmak@cmp.felk.cvut.cz)
39
40 #include "../precomp.hpp"
41 #include "../usac.hpp"
42 #if defined(HAVE_EIGEN)
43 #include <Eigen/Eigen>
44 #elif defined(HAVE_LAPACK)
45 #include "opencv_lapack.h"
46 #endif
47
48 namespace cv { namespace usac {
49 // This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
50 class DLSPnPImpl : public DLSPnP {
51 private:
52 const Mat * points_mat, * calib_norm_points_mat, * K_mat;
53 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
54 const Mat &K;
55 const float * const calib_norm_points, * const points;
56 #endif
57 public:
DLSPnPImpl(const Mat & points_,const Mat & calib_norm_points_,const Mat & K_)58 explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_) :
59 points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_)
60 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
61 , K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data)
62 #endif
63 {}
64 // return minimal sample size required for non-minimal estimation.
getMinimumRequiredSampleSize() const65 int getMinimumRequiredSampleSize() const override { return 3; }
66 // return maximum number of possible solutions.
getMaxNumberOfSolutions() const67 int getMaxNumberOfSolutions () const override { return 27; }
clone() const68 Ptr<NonMinimalSolver> clone () const override {
69 return makePtr<DLSPnPImpl>(*points_mat, *calib_norm_points_mat, *K_mat);
70 }
71 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
estimate(const std::vector<int> & sample,int sample_number,std::vector<Mat> & models_,const std::vector<double> &) const72 int estimate(const std::vector<int> &sample, int sample_number,
73 std::vector<Mat> &models_, const std::vector<double> &/*weights_*/) const override {
74 if (sample_number < getMinimumRequiredSampleSize())
75 return 0;
76
77 // Estimate the model parameters from the given point sample
78 // using weighted fitting if possible.
79
80 // Holds the normalized feature positions cross multiplied with itself
81 // i.e. n * n^t. This value is used multiple times so it is efficient to
82 // pre-compute it.
83 std::vector<Matx33d> normalized_feature_cross(sample_number);
84 std::vector<Vec3d> world_points(sample_number);
85 const Matx33d eye = Matx33d::eye();
86
87 // The bottom-right symmetric block matrix of inverse(A^T * A). Matrix H from
88 // Eq. 25 in the Appendix of the DLS paper.
89 Matx33d h_inverse = sample_number * eye;
90
91 // Compute V*W*b with the rotation parameters factored out. This is the
92 // translation parameterized by the 9 entries of the rotation matrix.
93 Matx<double, 3, 9> translation_factor = Matx<double, 3, 9>::zeros();
94
95 for (int i = 0; i < sample_number; i++) {
96 const int idx_world = 5 * sample[i], idx_calib = 3 * sample[i];
97 Vec3d normalized_feature_pos(calib_norm_points[idx_calib],
98 calib_norm_points[idx_calib+1],
99 calib_norm_points[idx_calib+2]);
100 normalized_feature_cross[i] = normalized_feature_pos * normalized_feature_pos.t();
101 world_points[i] = Vec3d(points[idx_world + 2], points[idx_world + 3], points[idx_world + 4]);
102
103 h_inverse -= normalized_feature_cross[i];
104 translation_factor += (normalized_feature_cross[i] - eye) * leftMultiplyMatrix(world_points[i]);
105 }
106
107 const Matx33d h_matrix = h_inverse.inv();
108 translation_factor = h_matrix * translation_factor;
109
110 // Compute the cost function J' of Eq. 17 in DLS paper. This is a factorized
111 // version where the rotation matrix parameters have been pulled out. The
112 // entries to this equation are the coefficients to the cost function which is
113 // a quartic in the rotation parameters.
114 Matx<double, 9, 9> ls_cost_coefficients = Matx<double, 9, 9>::zeros();
115 for (int i = 0; i < sample_number; i++)
116 ls_cost_coefficients +=
117 (leftMultiplyMatrix(world_points[i]) + translation_factor).t() *
118 (eye - normalized_feature_cross[i]) *
119 (leftMultiplyMatrix(world_points[i]) + translation_factor);
120
121 // Extract the coefficients of the jacobian (Eq. 18) from the
122 // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the
123 // rotation parameters. Each entry of the jacobian will be 0 at the roots of
124 // the polynomial, so we can arrange a system of polynomials from these
125 // equations.
126 double f1_coeff[20], f2_coeff[20], f3_coeff[20];
127 extractJacobianCoefficients(ls_cost_coefficients.val, f1_coeff, f2_coeff, f3_coeff);
128
129 // We create one equation with random terms that is generally non-zero at the
130 // roots of our system.
131 RNG rng;
132 const double macaulay_term[4] = { 100 * rng.uniform(-1.,1.), 100 * rng.uniform(-1.,1.),
133 100 * rng.uniform(-1.,1.), 100 * rng.uniform(-1.,1.) };
134
135 // Create Macaulay matrix that will be used to solve our polynonomial system.
136 Mat macaulay_matrix = Mat_<double>::zeros(120, 120);
137 createMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term, (double*)macaulay_matrix.data);
138
139 // Via the Schur complement trick, the top-left of the Macaulay matrix
140 // contains a multiplication matrix whose eigenvectors correspond to solutions
141 // to our system of equations.
142 Mat sol;
143 if (!solve(macaulay_matrix.colRange(27, 120).rowRange(27, 120),
144 macaulay_matrix.colRange(0 , 27).rowRange(27, 120), sol, DECOMP_LU))
145 return 0;
146
147 const Mat solution_polynomial = macaulay_matrix.colRange(0,27).rowRange(0,27) -
148 (macaulay_matrix.colRange(27,120).rowRange(0,27) * sol);
149
150 // Extract eigenvectors of the solution polynomial to obtain the roots which
151 // are contained in the entries of the eigenvectors.
152 #ifdef HAVE_EIGEN
153 Eigen::Map<Eigen::Matrix<double, 27, 27>> sol_poly((double*)solution_polynomial.data);
154 const Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(sol_poly);
155 const auto &eigen_vectors = eigen_solver.eigenvectors();
156 const auto &eigen_values = eigen_solver.eigenvalues();
157 #else
158 int mat_order = 27, info, lda = 27, ldvl = 1, ldvr = 27, lwork = 500;
159 double wr[27], wi[27] = {0}; // 27 = mat_order
160 std::vector<double> work(lwork), eig_vecs(729);
161 char jobvl = 'N', jobvr = 'V'; // only left eigen vectors are computed
162 dgeev_(&jobvl, &jobvr, &mat_order, (double*)solution_polynomial.data, &lda, wr, wi, nullptr, &ldvl,
163 &eig_vecs[0], &ldvr, &work[0], &lwork, &info);
164 if (info != 0) return 0;
165 #endif
166 models_ = std::vector<Mat>(); models_.reserve(3);
167 const int max_pts_to_eval = std::min(sample_number, 100);
168 std::vector<int> pts_random_shuffle(sample_number);
169 for (int i = 0; i < sample_number; i++)
170 pts_random_shuffle[i] = i;
171 randShuffle(pts_random_shuffle);
172
173 for (int i = 0; i < 27; i++) {
174 // If the rotation solutions are real, treat this as a valid candidate
175 // rotation.
176 // The first entry of the eigenvector should equal 1 according to our
177 // polynomial, so we must divide each solution by the first entry.
178
179 #ifdef HAVE_EIGEN
180 if (eigen_values(i).imag() != 0)
181 continue;
182 const double eigen_vec_1i = 1 / eigen_vectors(0, i).real();
183 const double s1 = eigen_vectors(9, i).real() * eigen_vec_1i,
184 s2 = eigen_vectors(3, i).real() * eigen_vec_1i,
185 s3 = eigen_vectors(1, i).real() * eigen_vec_1i;
186 #else
187 if (wi[i] != 0)
188 continue;
189 const double eigen_vec_1i = 1 / eig_vecs[mat_order*i];
190 const double s1 = eig_vecs[mat_order*i+9] * eigen_vec_1i,
191 s2 = eig_vecs[mat_order*i+3] * eigen_vec_1i,
192 s3 = eig_vecs[mat_order*i+1] * eigen_vec_1i;
193 #endif
194 // Compute the rotation (which is the transpose rotation of our solution)
195 // and translation.
196 const double qi = s1, qi2 = qi*qi, qj = s2, qj2 = qj*qj, qk = s3, qk2 = qk*qk;
197 const double s = 1 / (1 + qi2 + qj2 + qk2);
198 const Matx33d rot_mat (1-2*s*(qj2+qk2), 2*s*(qi*qj+qk), 2*s*(qi*qk-qj),
199 2*s*(qi*qj-qk), 1-2*s*(qi2+qk2), 2*s*(qj*qk+qi),
200 2*s*(qi*qk+qj), 2*s*(qj*qk-qi), 1-2*s*(qi2+qj2));
201 const Matx31d soln_translation = translation_factor * rot_mat.reshape<9,1>();
202
203 // Check that all points are in front of the camera. Discard the solution
204 // if this is not the case.
205 bool all_points_in_front_of_camera = true;
206 const Vec3d r3 (rot_mat(2,0),rot_mat(2,1),rot_mat(2,2));
207 const double z = soln_translation(2);
208 for (int pt = 0; pt < max_pts_to_eval; pt++) {
209 if (r3.dot(world_points[pts_random_shuffle[pt]]) + z < 0) {
210 all_points_in_front_of_camera = false;
211 break;
212 }
213 }
214
215 if (all_points_in_front_of_camera) {
216 Mat model;
217 hconcat(Math::rotVec2RotMat(Math::rotMat2RotVec(rot_mat)), soln_translation, model);
218 models_.emplace_back(K * model);
219 }
220 }
221 return static_cast<int>(models_.size());
222 #else
223 int estimate(const std::vector<int> &/*sample*/, int /*sample_number*/,
224 std::vector<Mat> &/*models_*/, const std::vector<double> &/*weights_*/) const override {
225 return 0;
226 #endif
227 }
228
229 protected:
230 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
231 const int indices[1968] = {
232 0, 35, 83, 118, 120, 121, 154, 155, 174, 203, 219, 238, 241, 242, 274, 275,
233 291, 294, 305, 323, 329, 339, 358, 360, 363, 395, 409, 436, 443, 478, 479,
234 481, 483, 484, 514, 515, 523, 529, 534, 551, 556, 563, 579, 580, 598, 599,
235 602, 604, 605, 634, 635, 641, 643, 649, 651, 654, 662, 665, 671, 676, 683,
236 689, 699, 700, 711, 718, 719, 723, 726, 750, 755, 769, 795, 796, 803, 827,
237 838, 839, 844, 846, 847, 870, 874, 875, 883, 885, 889, 894, 903, 911, 915,
238 916, 923, 939, 940, 947, 952, 958, 959, 965, 967, 968, 990, 994, 1001, 1003,
239 1005, 1006, 1009, 1011, 1014, 1022, 1023, 1025, 1026, 1031, 1035, 1036,
240 1049, 1059, 1060, 1062, 1067, 1071, 1072, 1079, 1080, 1089, 1115, 1116,
241 1163, 1164, 1168, 1198, 1201, 1209, 1210, 1233, 1234, 1235, 1236, 1254,
242 1259, 1283, 1284, 1288, 1299, 1317, 1318, 1322, 1330, 1331, 1348, 1353,
243 1354, 1355, 1356, 1371, 1374, 1377, 1379, 1385, 1403, 1404, 1408, 1409,
244 1419, 1434, 1437, 1438, 1443, 1449, 1452, 1475, 1476, 1479, 1489, 1516,
245 1519, 1523, 1524, 1528, 1536, 1558, 1559, 1564, 1570, 1572, 1573, 1593,
246 1594, 1595, 1596, 1599, 1603, 1607, 1609, 1614, 1619, 1620, 1631, 1636,
247 1639, 1643, 1644, 1648, 1650, 1656, 1659, 1660, 1677, 1678, 1679, 1685,
248 1691, 1693, 1694, 1708, 1713, 1714, 1716, 1719, 1721, 1722, 1723, 1727,
249 1729, 1731, 1734, 1736, 1737, 1739, 1740, 1742, 1745, 1751, 1756, 1759,
250 1764, 1768, 1769, 1770, 1776, 1779, 1780, 1786, 1791, 1794, 1797, 1799,
251 1806, 1812, 1815, 1829, 1830, 1835, 1836, 1839, 1849, 1874, 1875, 1876,
252 1879, 1883, 1884, 1888, 1894, 1896, 1907, 1918, 1919, 1927, 1933, 1935,
253 1936, 1949, 1950, 1953, 1954, 1956, 1959, 1963, 1964, 1965, 1967, 1969,
254 1974, 1979, 1980, 1983, 1988, 1991, 1994, 1995, 1996, 1999, 2004, 2008,
255 2010, 2014, 2016, 2017, 2019, 2020, 2027, 2032, 2037, 2039, 2048, 2054,
256 2056, 2057, 2068, 2069, 2070, 2073, 2079, 2081, 2082, 2083, 2084, 2085,
257 2086, 2087, 2091, 2096, 2097, 2099, 2100, 2102, 2103, 2105, 2106, 2108,
258 2111, 2114, 2115, 2119, 2129, 2130, 2134, 2136, 2137, 2140, 2142, 2146,
259 2147, 2151, 2152, 2154, 2157, 2169, 2178, 2195, 2196, 2213, 2242, 2243,
260 2244, 2247, 2248, 2278, 2290, 2298, 2299, 2312, 2313, 2314, 2315, 2316,
261 2333, 2334, 2339, 2341, 2362, 2363, 2364, 2367, 2368, 2379, 2396, 2397,
262 2398, 2411, 2419, 2420, 2427, 2428, 2432, 2433, 2434, 2436, 2451, 2453,
263 2454, 2455, 2457, 2459, 2461, 2465, 2482, 2484, 2487, 2488, 2489, 2499,
264 2513, 2514, 2516, 2517, 2532, 2538, 2541, 2555, 2556, 2558, 2559, 2569,
265 2573, 2596, 2598, 2599, 2602, 2603, 2604, 2607, 2608, 2612, 2616, 2638,
266 2639, 2653, 2659, 2661, 2662, 2672, 2673, 2674, 2676, 2678, 2679, 2680,
267 2683, 2687, 2689, 2693, 2694, 2699, 2700, 2701, 2711, 2712, 2716, 2718,
268 2719, 2722, 2724, 2727, 2728, 2730, 2732, 2735, 2736, 2739, 2740, 2756,
269 2757, 2759, 2774, 2780, 2782, 2783, 2787, 2788, 2792, 2793, 2798, 2799,
270 2800, 2801, 2802, 2803, 2807, 2811, 2813, 2815, 2816, 2817, 2819, 2820,
271 2821, 2822, 2825, 2831, 2832, 2838, 2839, 2842, 2847, 2849, 2850, 2852,
272 2855, 2856, 2860, 2866, 2871, 2873, 2874, 2876, 2877, 2895, 2901, 2904,
273 2909, 2910, 2916, 2918, 2919, 2929, 2932, 2933, 2953, 2954, 2955, 2956,
274 2958, 2959, 2962, 2964, 2967, 2968, 2972, 2973, 2974, 2976, 2987, 2999,
275 3016, 3022, 3024, 3025, 3029, 3030, 3032, 3033, 3038, 3039, 3040, 3043,
276 3044, 3045, 3047, 3052, 3053, 3059, 3060, 3061, 3063, 3068, 3071, 3072,
277 3073, 3074, 3075, 3078, 3079, 3082, 3087, 3090, 3092, 3093, 3094, 3095,
278 3096, 3097, 3100, 3107, 3112, 3116, 3117, 3137, 3143, 3145, 3146, 3147,
279 3148, 3149, 3152, 3158, 3160, 3161, 3162, 3164, 3165, 3166, 3167, 3172,
280 3175, 3176, 3177, 3180, 3181, 3182, 3183, 3186, 3188, 3192, 3193, 3194,
281 3198, 3210, 3212, 3213, 3214, 3215, 3217, 3222, 3226, 3231, 3232, 3233,
282 3234, 3236, 3255, 3269, 3270, 3276, 3279, 3289, 3309, 3310, 3314, 3315,
283 3316, 3319, 3324, 3328, 3331, 3334, 3336, 3347, 3350, 3359, 3366, 3390,
284 3395, 3409, 3429, 3435, 3436, 3443, 3467, 3470, 3478, 3479, 3504, 3509,
285 3510, 3518, 3519, 3532, 3533, 3549, 3550, 3553, 3554, 3555, 3558, 3559,
286 3562, 3567, 3571, 3572, 3573, 3574, 3576, 3587, 3590, 3637, 3648, 3652,
287 3670, 3673, 3677, 3681, 3685, 3691, 3693, 3698, 3749, 3757, 3758, 3770,
288 3772, 3789, 3790, 3793, 3794, 3797, 3798, 3800, 3806, 3811, 3812, 3813,
289 3814, 3818, 3830, 3888, 3890, 3893, 3920, 3921, 3922, 3925, 3926, 3927,
290 3989, 3990, 3999, 4024, 4029, 4030, 4034, 4035, 4039, 4051, 4054, 4056,
291 4063, 4067, 4070, 4109, 4118, 4132, 4144, 4149, 4150, 4153, 4154, 4158,
292 4171, 4172, 4173, 4174, 4183, 4190, 4237, 4252, 4264, 4270, 4273, 4277,
293 4291, 4293, 4298, 4303, 4325, 4354, 4361, 4363, 4369, 4371, 4374, 4382,
294 4385, 4391, 4396, 4409, 4419, 4420, 4421, 4429, 4431, 4439, 4442, 4474,
295 4475, 4491, 4494, 4505, 4523, 4529, 4539, 4549, 4558, 4590, 4609, 4624,
296 4629, 4635, 4636, 4663, 4667, 4670, 4679, 4708, 4713, 4731, 4737, 4739,
297 4745, 4769, 4785, 4788, 4789, 4794, 4797, 4827, 4828, 4832, 4855, 4857,
298 4861, 4905, 4908, 4909, 4913, 4914, 4916, 4950, 4984, 4989, 4995, 5023,
299 5027, 5030, 5067, 5071, 5095, 5098, 5145, 5148, 5153, 5155, 5189, 5224,
300 5229, 5230, 5234, 5251, 5254, 5263, 5270, 5308, 5337, 5385, 5388, 5389,
301 5394, 5427, 5455, 5505, 5508, 5513, 5572, 5584, 5590, 5593, 5611, 5613,
302 5623, 5680, 5684, 5692, 5704, 5707, 5708, 5710, 5712, 5713, 5731, 5733,
303 5735, 5737, 5743, 5744, 5790, 5803, 5805, 5823, 5824, 5827, 5829, 5831,
304 5835, 5860, 5863, 5864, 5867, 5870, 5872, 5921, 5925, 5926, 5942, 5943,
305 5946, 5981, 5982, 5985, 5989, 5991, 5992, 6041, 6062, 6101, 6105, 6109,
306 6111, 6184, 6190, 6211, 6223, 6281, 6285, 6286, 6302, 6303, 6306, 6307,
307 6309, 6341, 6342, 6344, 6349, 6350, 6351, 6352, 6424, 6429, 6463, 6470,
308 6585, 6589, 6644, 6664, 6667, 6668, 6670, 6691, 6697, 6703, 6704, 6825,
309 6828, 6904, 6907, 6943, 6944, 7006, 7024, 7026, 7027, 7062, 7063, 7064,
310 7088, 7110, 7121, 7123, 7125, 7126, 7131, 7142, 7143, 7145, 7146, 7151,
311 7155, 7169, 7180, 7181, 7182, 7187, 7189, 7191, 7192, 7208, 7230, 7241,
312 7243, 7245, 7246, 7251, 7262, 7263, 7265, 7266, 7267, 7269, 7271, 7275,
313 7289, 7300, 7302, 7304, 7307, 7310, 7311, 7312, 7362, 7376, 7421, 7425,
314 7426, 7428, 7504, 7543, 7665, 7726, 7746, 7747, 7781, 7782, 7784, 7785,
315 7846, 7864, 7866, 7867, 7901, 7902, 7903, 7904, 7966, 7986, 8021, 8022,
316 8025, 8141, 8145, 8201, 8203, 8211, 8222, 8225, 8231, 8249, 8260, 8261,
317 8265, 8269, 8271, 8317, 8328, 8332, 8353, 8357, 8361, 8365, 8373, 8378,
318 8420, 8427, 8428, 8431, 8432, 8433, 8450, 8451, 8453, 8455, 8457, 8458,
319 8459, 8461, 8465, 8480, 8482, 8486, 8487, 8489, 8513, 8514, 8515, 8516,
320 8517, 8565, 8583, 8584, 8587, 8589, 8623, 8624, 8630, 8632, 8681, 8685,
321 8686, 8702, 8703, 8704, 8706, 8707, 8709, 8742, 8743, 8744, 8750, 8751,
322 8752, 8808, 8810, 8840, 8841, 8845, 8846, 8905, 8909, 8912, 8918, 8920,
323 8924, 8925, 8927, 8932, 8940, 8941, 8943, 8947, 8948, 8949, 8950, 8952,
324 8953, 8954, 8958, 8970, 8971, 8972, 8973, 8974, 8975, 8977, 8984, 8990,
325 8992, 8996, 9021, 9036, 9037, 9038, 9039, 9049, 9050, 9053, 9076, 9077,
326 9078, 9079, 9080, 9082, 9084, 9086, 9087, 9088, 9092, 9096, 9098, 9119,
327 9168, 9201, 9205, 9274, 9291, 9294, 9305, 9329, 9339, 9345, 9349, 9387,
328 9391, 9397, 9400, 9402, 9415, 9416, 9418, 9432, 9437, 9455, 9458, 9461,
329 9466, 9468, 9473, 9475, 9522, 9524, 9526, 9536, 9546, 9548, 9577, 9581,
330 9582, 9585, 9586, 9588, 9614, 9628, 9633, 9639, 9641, 9642, 9643, 9647,
331 9651, 9656, 9657, 9659, 9660, 9662, 9665, 9671, 9679, 9689, 9690, 9696,
332 9700, 9701, 9706, 9708, 9709, 9711, 9714, 9717, 9751, 9752, 9757, 9758,
333 9760, 9767, 9768, 9770, 9778, 9780, 9781, 9792, 9797, 9798, 9800, 9801,
334 9805, 9806, 9810, 9812, 9815, 9818, 9835, 9836, 9869, 9884, 9885, 9887,
335 9900, 9903, 9904, 9907, 9908, 9909, 9910, 9914, 9930, 9931, 9934, 9937,
336 9943, 9944, 9950, 9952, 9986, 9987, 9991, 9997, 10000, 10002, 10004, 10006,
337 10012, 10015, 10016, 10018, 10026, 10028, 10032, 10033, 10037, 10053, 10055,
338 10057, 10058, 10062, 10066, 10073, 10075, 10096, 10109, 10110, 10113, 10119,
339 10123, 10124, 10125, 10127, 10139, 10140, 10143, 10147, 10148, 10149, 10150,
340 10151, 10154, 10155, 10159, 10170, 10171, 10174, 10176, 10177, 10180, 10184,
341 10187, 10190, 10192, 10197, 10225, 10229, 10231, 10232, 10237, 10238, 10240,
342 10244, 10245, 10247, 10250, 10252, 10258, 10260, 10261, 10263, 10268, 10272,
343 10273, 10274, 10277, 10278, 10280, 10286, 10290, 10292, 10293, 10294, 10295,
344 10297, 10298, 10312, 10315, 10316, 10351, 10357, 10360, 10364, 10368, 10372,
345 10378, 10388, 10392, 10393, 10397, 10401, 10405, 10413, 10415, 10417, 10418,
346 10435, 10462, 10471, 10472, 10473, 10477, 10478, 10479, 10480, 10483, 10487,
347 10490, 10493, 10498, 10499, 10500, 10501, 10511, 10512, 10517, 10518, 10519,
348 10520, 10522, 10526, 10527, 10530, 10532, 10535, 10536, 10538, 10540, 10555,
349 10556, 10557, 10587, 10591, 10597, 10600, 10602, 10608, 10615, 10616, 10618,
350 10632, 10637, 10641, 10645, 10655, 10658, 10666, 10673, 10675, 10711, 10717,
351 10720, 10724, 10732, 10738, 10747, 10748, 10750, 10752, 10753, 10757, 10771,
352 10773, 10775, 10777, 10778, 10784, 10795, 10827, 10840, 10842, 10855, 10856,
353 10872, 10895, 10901, 10905, 10906, 10908, 10913, 10943, 10947, 10948, 10951,
354 10952, 10957, 10958, 10960, 10961, 10962, 10967, 10970, 10975, 10976, 10977,
355 10978, 10980, 10981, 10982, 10992, 10997, 10998, 11000, 11006, 11010, 11012,
356 11015, 11018, 11026, 11031, 11033, 11034, 11035, 11036, 11057, 11068, 11069,
357 11081, 11082, 11084, 11085, 11086, 11087, 11096, 11097, 11100, 11102, 11103,
358 11106, 11108, 11114, 11130, 11134, 11137, 11141, 11142, 11146, 11148, 11149,
359 11151, 11152, 11154, 11177, 11188, 11189, 11201, 11202, 11204, 11205, 11206,
360 11207, 11216, 11217, 11220, 11222, 11223, 11226, 11227, 11228, 11229, 11230,
361 11234, 11250, 11251, 11254, 11257, 11262, 11264, 11266, 11270, 11271, 11272,
362 11274, 11311, 11317, 11320, 11328, 11338, 11352, 11357, 11361, 11365, 11375,
363 11378, 11395, 11426, 11427, 11440, 11442, 11444, 11446, 11452, 11455, 11456,
364 11466, 11468, 11472, 11473, 11493, 11495, 11497, 11501, 11502, 11506, 11508,
365 11513, 11543, 11547, 11548, 11552, 11558, 11560, 11561, 11562, 11567, 11575,
366 11576, 11577, 11580, 11581, 11582, 11592, 11598, 11610, 11612, 11615, 11621,
367 11626, 11628, 11629, 11631, 11633, 11634, 11636, 11682, 11684, 11686, 11696,
368 11706, 11707, 11708, 11710, 11731, 11737, 11741, 11742, 11744, 11746, 11748,
369 11788, 11801, 11802, 11807, 11816, 11817, 11820, 11822, 11850, 11861, 11865,
370 11866, 11868, 11869, 11871, 11874, 11922, 11924, 11926, 11936, 11944, 11946,
371 11947, 11948, 11950, 11971, 11977, 11982, 11983, 11984, 11986, 12051, 12065,
372 12089, 12105, 12109, 12157, 12158, 12159, 12168, 12170, 12173, 12197, 12198,
373 12199, 12200, 12201, 12202, 12205, 12206, 12207, 12212, 12216, 12218, 12277,
374 12278, 12288, 12290, 12317, 12318, 12320, 12321, 12325, 12326, 12332, 12338,
375 12397, 12408, 12437, 12441, 12445, 12458, 12491, 12508, 12513, 12514, 12516,
376 12531, 12534, 12537, 12539, 12545, 12564, 12568, 12569, 12579, 12588, 12589,
377 12594, 12597, 12620, 12627, 12628, 12632, 12633, 12651, 12653, 12655, 12657,
378 12659, 12661, 12665, 12682, 12687, 12689, 12708, 12709, 12713, 12714, 12716,
379 12717, 12747, 12748, 12751, 12752, 12770, 12775, 12777, 12778, 12781, 12800,
380 12806, 12828, 12829, 12833, 12834, 12835, 12836, 12867, 12871, 12888, 12895,
381 12898, 12921, 12925, 12948, 12953, 12955, 12996, 13008, 13010, 13013, 13040,
382 13041, 13042, 13044, 13045, 13046, 13047, 13048, 13106, 13107, 13120, 13122,
383 13124, 13126, 13132, 13135, 13136, 13146, 13147, 13148, 13150, 13152, 13153,
384 13171, 13173, 13175, 13177, 13182, 13184, 13186, 13193, 13207, 13230, 13234,
385 13243, 13245, 13249, 13254, 13263, 13267, 13269, 13271, 13275, 13276, 13299,
386 13300, 13304, 13307, 13310, 13312, 13319, 13338, 13355, 13356, 13370, 13373,
387 13400, 13402, 13403, 13404, 13406, 13407, 13408, 13438, 13459, 13471, 13472,
388 13473, 13474, 13476, 13490, 13493, 13494, 13498, 13499, 13501, 13520, 13522,
389 13524, 13526, 13527, 13528, 13539, 13555, 13556, 13557, 13591, 13592, 13593,
390 13608, 13610, 13613, 13618, 13619, 13621, 13640, 13641, 13642, 13645, 13646,
391 13647, 13675, 13676, 13677, 13711, 13712, 13728, 13730, 13738, 13741, 13760,
392 13761, 13765, 13766, 13795, 13796, 13831, 13848, 13858, 13881, 13885, 13915,
393 13944, 13949, 13950, 13957, 13958, 13959, 13970, 13972, 13973, 13993, 13994,
394 13995, 13997, 13998, 13999, 14000, 14002, 14006, 14007, 14012, 14013, 14014,
395 14016, 14018, 14027, 14069, 14077, 14078, 14088, 14090, 14092, 14113, 14114,
396 14117, 14118, 14120, 14121, 14125, 14126, 14132, 14133, 14134, 14138, 14187,
397 14188, 14191, 14192, 14208, 14210, 14215, 14217, 14218, 14221, 14240, 14241,
398 14245, 14246, 14273, 14274, 14275, 14276, 14307, 14311, 14328, 14335, 14338,
399 14361, 14365, 14393, 14395
400 };
401 void createMacaulayMatrix(const double a[20], const double b[20],
402 const double c[20], const double u[4], double * macaulay_matrix) const {
403 // The matrix is very large (14400 elements!) and sparse (1968 non-zero
404 // elements) so we load it from pre-computed values calculated in matlab.
405
406 const double values[1968] = {
407 u[0], a[0], b[0], c[0], u[3], u[0], a[0], a[9], b[0], b[9], c[0], c[9],
408 u[3], u[0], a[9], a[13], a[0], b[9], b[0], b[13], c[0], c[9], c[13], u[2],
409 u[0], a[10], a[0], b[0], b[10], c[10], c[0], u[2], u[3], u[0], a[10], a[4],
410 a[0], a[9], b[10], b[0], b[9], b[4], c[10], c[0], c[4], c[9], u[2], u[3],
411 u[0], a[4], a[11], a[0], a[9], a[13], a[10], b[4], b[0], b[10], b[9], b[13],
412 b[11], c[10], c[4], c[9], c[0], c[11], c[13], u[2], u[0], a[0], a[14],
413 a[10], b[0], b[10], b[14], c[0], c[14], c[10], u[2], u[3], u[0], a[9],
414 a[14], a[5], a[10], a[0], a[4], b[14], b[0], b[10], b[9], b[4], b[5], c[14],
415 c[10], c[9], c[0], c[5], c[4], u[2], u[3], u[0], a[13], a[5], a[10], a[4],
416 a[9], a[0], a[11], a[14], b[5], b[10], b[9], b[14], b[0], b[4], b[13],
417 b[11], c[14], c[5], c[4], c[0], c[13], c[10], c[9], c[11], u[1], u[0], a[8],
418 a[0], b[8], b[0], c[0], c[8], u[1], u[3], u[0], a[0], a[8], a[3], a[9],
419 b[8], b[0], b[3], b[9], c[9], c[8], c[0], c[3], u[1], u[3], u[0], a[0],
420 a[9], a[3], a[7], a[13], a[8], b[3], b[0], b[9], b[8], b[7], b[13], c[13],
421 c[8], c[3], c[0], c[9], c[7], u[1], u[2], u[0], a[2], a[10], a[0], a[8],
422 b[8], b[0], b[2], b[10], c[10], c[0], c[2], c[8], u[1], u[2], u[3], u[0],
423 a[10], a[2], a[16], a[4], a[9], a[8], a[0], a[3], b[2], b[10], b[0], b[8],
424 b[3], b[9], b[16], b[4], c[4], c[0], c[9], c[2], c[8], c[10], c[16], c[3],
425 u[1], u[2], u[3], u[0], a[10], a[4], a[16], a[11], a[13], a[8], a[0], a[3],
426 a[9], a[7], a[2], b[16], b[0], b[10], b[4], b[9], b[8], b[2], b[3], b[7],
427 b[13], b[11], c[11], c[2], c[9], c[13], c[16], c[3], c[0], c[8], c[10],
428 c[4], c[7], u[1], u[2], u[0], a[0], a[8], a[17], a[14], a[10], a[2], b[0],
429 b[8], b[2], b[10], b[17], b[14], c[14], c[0], c[10], c[8], c[17], c[2],
430 u[1], u[2], u[3], u[0], a[9], a[3], a[14], a[17], a[5], a[4], a[2], a[0],
431 a[8], a[10], a[16], b[17], b[14], b[10], b[8], b[0], b[2], b[9], b[3],
432 b[16], b[4], b[5], c[5], c[10], c[9], c[4], c[0], c[17], c[2], c[3], c[8],
433 c[14], c[16], u[1], u[2], u[3], u[0], a[14], a[13], a[7], a[5], a[11], a[2],
434 a[10], a[16], a[9], a[3], a[8], a[4], a[17], b[10], b[14], b[5], b[4], b[2],
435 b[3], b[17], b[8], b[9], b[16], b[13], b[7], b[11], c[17], c[4], c[13],
436 c[11], c[9], c[16], c[8], c[10], c[7], c[2], c[3], c[14], c[5], u[1], u[0],
437 a[12], a[8], a[0], b[0], b[12], b[8], c[0], c[8], c[12], u[1], u[3], u[0],
438 a[0], a[8], a[12], a[18], a[3], a[9], b[12], b[8], b[0], b[9], b[18], b[3],
439 c[9], c[3], c[12], c[0], c[8], c[18], u[1], u[3], u[0], a[0], a[8], a[9],
440 a[3], a[18], a[7], a[12], a[13], b[18], b[0], b[8], b[3], b[9], b[12],
441 b[13], b[7], c[13], c[7], c[12], c[18], c[0], c[8], c[9], c[3], u[1], u[2],
442 u[0], a[1], a[2], a[0], a[8], a[12], a[10], b[12], b[0], b[8], b[10], b[1],
443 b[2], c[10], c[2], c[0], c[8], c[1], c[12], u[1], u[2], u[3], u[0], a[10],
444 a[2], a[1], a[16], a[9], a[3], a[0], a[12], a[8], a[18], a[4], b[1], b[2],
445 b[8], b[10], b[12], b[0], b[18], b[9], b[3], b[4], b[16], c[4], c[16], c[8],
446 c[9], c[0], c[3], c[1], c[12], c[10], c[2], c[18], u[1], u[2], u[3], u[0],
447 a[10], a[2], a[4], a[16], a[13], a[7], a[9], a[12], a[8], a[18], a[3], a[1],
448 a[11], b[10], b[8], b[2], b[16], b[3], b[4], b[12], b[1], b[18], b[9],
449 b[13], b[7], b[11], c[11], c[1], c[3], c[13], c[9], c[7], c[18], c[8],
450 c[12], c[10], c[2], c[4], c[16], u[1], u[2], u[0], a[8], a[12], a[17],
451 a[10], a[2], a[1], a[0], a[14], b[0], b[8], b[12], b[1], b[10], b[2], b[14],
452 b[17], c[14], c[17], c[10], c[0], c[8], c[2], c[12], c[1], u[1], u[2], u[3],
453 u[0], a[3], a[18], a[14], a[17], a[4], a[16], a[10], a[1], a[8], a[12],
454 a[2], a[9], a[5], b[17], b[2], b[14], b[12], b[8], b[1], b[10], b[9], b[3],
455 b[18], b[4], b[16], b[5], c[5], c[2], c[4], c[9], c[3], c[10], c[16], c[8],
456 c[1], c[18], c[12], c[14], c[17], u[1], u[2], u[3], u[0], a[14], a[17],
457 a[7], a[5], a[11], a[4], a[1], a[2], a[3], a[18], a[12], a[16], a[13],
458 b[14], b[2], b[17], b[16], b[5], b[1], b[18], b[12], b[3], b[4], b[13],
459 b[7], b[11], c[16], c[11], c[13], c[7], c[4], c[3], c[12], c[2], c[1],
460 c[18], c[14], c[17], c[5], u[2], a[10], a[2], a[6], a[14], a[17], b[8],
461 b[0], b[10], b[2], b[17], b[14], b[6], c[6], c[0], c[10], c[14], c[2], c[8],
462 c[17], u[2], a[10], a[6], a[14], b[0], b[10], b[14], b[6], c[10], c[0],
463 c[6], c[14], u[2], a[2], a[1], a[14], a[17], a[10], a[6], b[12], b[8],
464 b[10], b[2], b[1], b[14], b[17], b[6], c[6], c[8], c[14], c[10], c[2],
465 c[17], c[1], c[12], a[17], a[6], a[1], b[19], b[1], b[17], b[6], c[6],
466 c[19], c[1], c[17], a[1], a[14], a[17], a[6], a[2], b[19], b[12], b[2],
467 b[1], b[14], b[17], b[6], c[6], c[12], c[17], c[2], c[1], c[14], c[19],
468 a[8], a[12], a[19], b[12], b[8], b[19], c[8], c[12], c[19], a[14], a[17],
469 a[6], b[8], b[2], b[10], b[14], b[17], b[6], c[10], c[14], c[6], c[8],
470 c[17], c[2], a[17], a[6], a[14], b[12], b[1], b[2], b[14], b[17], b[6],
471 c[2], c[6], c[14], c[17], c[12], c[1], a[6], a[17], b[19], b[1], b[17],
472 b[6], c[1], c[17], c[6], c[19], u[3], a[11], a[9], a[13], a[15], a[4],
473 b[11], b[9], b[4], b[13], b[15], c[4], c[11], c[13], c[0], c[10], c[9],
474 c[15], u[3], a[13], a[15], a[9], b[13], b[9], b[15], c[9], c[13], c[0],
475 c[15], a[14], a[6], b[0], b[10], b[14], b[6], c[0], c[14], c[10], c[6],
476 a[13], a[15], a[7], b[13], b[15], b[7], c[7], c[8], c[9], c[3], c[13],
477 c[15], a[13], a[7], a[15], b[13], b[7], b[15], c[12], c[3], c[18], c[13],
478 c[7], c[15], a[6], b[10], b[14], b[6], c[10], c[6], c[14], a[7], a[15],
479 b[7], b[15], c[19], c[18], c[7], c[15], a[6], b[2], b[17], b[14], b[6],
480 c[14], c[6], c[2], c[17], a[15], b[15], c[3], c[13], c[7], c[15], a[15],
481 b[15], c[18], c[7], c[15], a[6], b[1], b[17], b[6], c[17], c[6], c[1], a[6],
482 a[17], a[5], b[18], b[1], b[17], b[16], b[6], b[5], c[16], c[5], c[6],
483 c[17], c[18], c[1], a[5], a[6], a[14], b[14], b[9], b[10], b[4], b[6], b[5],
484 c[6], c[9], c[10], c[5], c[4], c[14], a[11], a[15], a[13], b[11], b[15],
485 b[13], c[4], c[13], c[14], c[5], c[11], c[15], a[15], b[15], c[13], c[4],
486 c[11], c[15], b[17], b[6], c[6], c[17], a[5], a[11], a[4], b[5], b[11],
487 b[4], b[13], b[15], c[14], c[4], c[13], c[6], c[15], c[5], c[11], b[14],
488 b[6], c[14], c[6], c[13], c[15], a[6], b[16], b[17], b[6], b[5], c[5], c[6],
489 c[16], c[17], c[7], c[15], b[5], b[6], c[5], c[6], a[6], b[11], b[6], b[5],
490 c[6], c[11], c[5], u[3], a[15], a[4], a[11], a[13], a[9], a[5], b[4], b[13],
491 b[5], b[9], b[11], b[15], c[5], c[11], c[10], c[9], c[15], c[14], c[4],
492 c[13], u[2], a[11], a[14], a[5], a[4], a[10], a[6], b[14], b[4], b[6],
493 b[10], b[9], b[13], b[5], b[11], c[6], c[5], c[10], c[9], c[11], c[13],
494 c[14], c[4], a[15], b[15], c[7], c[16], c[15], c[11], b[6], c[6], c[15],
495 a[11], b[11], b[15], c[5], c[11], c[15], c[6], a[5], b[15], b[5], b[11],
496 c[6], c[5], c[15], c[11], a[15], b[15], c[11], c[15], c[5], c[15], c[11],
497 a[13], a[15], a[11], b[13], b[11], b[15], c[11], c[15], c[9], c[10], c[4],
498 c[13], a[1], a[17], a[19], b[19], b[1], b[17], c[17], c[19], c[1], u[1],
499 a[8], a[12], a[9], a[3], a[18], a[13], a[19], a[7], b[8], b[12], b[9],
500 b[18], b[3], b[19], b[13], b[7], c[13], c[7], c[19], c[8], c[12], c[9],
501 c[3], c[18], a[6], b[6], b[4], b[14], b[5], c[4], c[14], c[5], c[6], a[6],
502 a[5], a[14], b[6], b[5], b[13], b[14], b[4], b[11], c[14], c[13], c[4],
503 c[11], c[6], c[5], a[12], a[19], b[19], b[12], c[12], c[19], u[2], a[16],
504 a[6], a[5], a[14], a[2], a[1], a[17], a[4], b[17], b[6], b[1], b[12], b[2],
505 b[18], b[3], b[14], b[4], b[16], b[5], c[17], c[3], c[5], c[4], c[16],
506 c[14], c[2], c[12], c[18], c[1], c[6], u[1], a[1], a[0], a[8], a[12], a[19],
507 a[10], a[2], b[19], b[0], b[8], b[12], b[10], b[2], b[1], c[10], c[2], c[1],
508 c[8], c[12], c[0], c[19], a[19], b[19], c[19], a[15], a[13], b[15], b[13],
509 c[13], c[15], c[0], c[9], a[16], a[11], a[15], a[7], a[18], b[16], b[18],
510 b[11], b[7], b[15], c[7], c[15], c[19], c[18], c[1], c[16], c[11], a[11],
511 a[15], a[7], b[11], b[7], b[15], c[15], c[16], c[7], c[17], c[11], c[5],
512 u[3], a[4], a[11], a[15], a[3], a[9], a[7], a[13], a[16], b[9], b[4], b[11],
513 b[13], b[3], b[16], b[7], b[15], c[16], c[13], c[15], c[7], c[8], c[9],
514 c[10], c[2], c[3], c[4], c[11], a[2], a[1], a[3], a[18], a[12], a[19], a[4],
515 a[16], b[2], b[19], b[1], b[12], b[3], b[18], b[16], b[4], c[4], c[16],
516 c[19], c[18], c[12], c[3], c[2], c[1], a[5], a[14], a[17], a[6], b[6],
517 b[17], b[3], b[2], b[14], b[16], b[4], b[5], c[6], c[4], c[5], c[14], c[3],
518 c[2], c[16], c[17], u[1], a[17], a[5], a[11], a[16], a[1], a[18], a[19],
519 a[7], b[17], b[1], b[5], b[19], b[18], b[16], b[7], b[11], c[7], c[16],
520 c[18], c[11], c[19], c[1], c[17], c[5], u[2], a[4], a[16], a[6], a[5],
521 a[17], a[10], a[2], a[14], b[6], b[14], b[2], b[8], b[10], b[3], b[9],
522 b[17], b[4], b[16], b[5], c[14], c[9], c[4], c[5], c[10], c[17], c[8],
523 c[16], c[3], c[2], c[6], u[1], a[18], a[14], a[17], a[4], a[16], a[2],
524 a[12], a[19], a[1], a[5], a[3], b[14], b[1], b[17], b[19], b[12], b[2],
525 b[3], b[18], b[4], b[16], b[5], c[5], c[1], c[16], c[3], c[18], c[2], c[12],
526 c[4], c[19], c[14], c[17], a[17], a[16], a[1], a[19], a[5], a[18], b[17],
527 b[19], b[1], b[18], b[16], b[5], c[5], c[18], c[1], c[19], c[16], c[17],
528 u[1], a[10], a[2], a[1], a[9], a[3], a[18], a[8], a[19], a[12], a[4], a[16],
529 b[10], b[1], b[12], b[2], b[19], b[8], b[9], b[3], b[18], b[4], b[16], c[4],
530 c[16], c[12], c[3], c[8], c[18], c[9], c[19], c[10], c[2], c[1], a[1],
531 a[16], a[7], a[18], a[19], a[11], b[1], b[19], b[16], b[18], b[7], b[11],
532 c[11], c[18], c[7], c[19], c[1], c[16], a[6], a[5], a[17], a[1], a[16],
533 b[6], b[19], b[1], b[18], b[17], b[16], b[5], c[18], c[16], c[17], c[1],
534 c[5], c[19], c[6], a[11], a[15], a[7], b[11], b[7], b[15], c[15], c[18],
535 c[1], c[7], c[16], c[11], u[1], a[2], a[1], a[4], a[16], a[13], a[7], a[3],
536 a[19], a[12], a[18], a[11], b[2], b[12], b[1], b[4], b[18], b[16], b[19],
537 b[3], b[13], b[7], b[11], c[11], c[18], c[7], c[3], c[13], c[12], c[19],
538 c[2], c[1], c[4], c[16], u[3], a[5], a[15], a[16], a[4], a[13], a[7], a[3],
539 a[11], b[4], b[5], b[11], b[16], b[7], b[3], b[13], b[15], c[11], c[15],
540 c[13], c[2], c[3], c[4], c[14], c[17], c[16], c[7], c[5], u[2], a[6], a[11],
541 a[17], a[14], a[4], a[16], a[2], a[5], b[14], b[6], b[5], b[17], b[16],
542 b[2], b[3], b[4], b[7], b[13], b[11], c[5], c[13], c[11], c[4], c[2], c[3],
543 c[14], c[7], c[17], c[16], c[6], a[1], a[18], a[19], a[16], b[1], b[19],
544 b[18], b[16], c[16], c[19], c[18], c[1], u[3], a[5], a[11], a[16], a[7],
545 a[18], a[15], b[5], b[16], b[18], b[7], b[11], b[15], c[15], c[11], c[7],
546 c[1], c[18], c[16], c[17], c[5], u[3], a[4], a[16], a[11], a[15], a[13],
547 a[18], a[3], a[7], b[4], b[3], b[16], b[7], b[11], b[18], b[13], b[15],
548 c[7], c[15], c[13], c[12], c[3], c[2], c[1], c[18], c[4], c[16], c[11],
549 a[5], a[11], a[16], b[5], b[16], b[7], b[11], b[15], c[15], c[11], c[17],
550 c[16], c[7], c[5], c[6], a[11], a[7], a[13], a[15], b[13], b[11], b[15],
551 b[7], c[15], c[3], c[2], c[13], c[4], c[16], c[7], c[11], a[6], a[5], a[17],
552 b[6], b[7], b[17], b[16], b[5], b[11], c[11], c[5], c[17], c[7], c[16],
553 c[6], a[15], b[15], c[15], c[9], c[13], a[8], a[12], a[19], a[10], a[2],
554 a[1], b[8], b[12], b[19], b[2], b[10], b[1], c[10], c[2], c[1], c[12],
555 c[19], c[8], a[12], a[19], a[2], a[1], b[12], b[19], b[1], b[2], c[2], c[1],
556 c[19], c[12], a[19], a[1], b[19], b[1], c[1], c[19], u[3], a[9], a[13],
557 a[7], a[15], a[3], b[7], b[9], b[13], b[3], b[15], c[15], c[3], c[7], c[0],
558 c[8], c[9], c[13], u[3], a[9], a[3], a[13], a[7], a[18], a[15], b[9], b[3],
559 b[7], b[13], b[18], b[15], c[15], c[18], c[8], c[12], c[9], c[3], c[13],
560 c[7], a[3], a[18], a[13], a[7], a[15], b[3], b[18], b[13], b[7], b[15],
561 c[15], c[12], c[19], c[3], c[18], c[13], c[7], a[18], a[7], a[15], b[18],
562 b[7], b[15], c[15], c[19], c[18], c[7], a[19], a[0], a[8], a[12], b[8],
563 b[0], b[12], b[19], c[0], c[8], c[12], c[19], u[2], a[6], a[5], a[17],
564 a[16], a[1], a[11], b[6], b[17], b[1], b[18], b[16], b[7], b[5], b[11],
565 c[7], c[11], c[5], c[16], c[1], c[18], c[17], c[6], u[2], a[4], a[6], a[14],
566 a[10], a[5], b[6], b[10], b[0], b[9], b[14], b[4], b[5], c[6], c[14], c[0],
567 c[4], c[9], c[10], c[5], u[1], a[19], a[12], a[0], a[8], b[0], b[8], b[19],
568 b[12], c[0], c[8], c[12], c[19], u[1], a[0], a[8], a[12], a[19], a[18],
569 a[9], a[3], b[19], b[0], b[12], b[8], b[9], b[3], b[18], c[9], c[3], c[18],
570 c[19], c[0], c[8], c[12], a[8], a[12], a[19], a[9], a[3], a[18], b[8],
571 b[19], b[12], b[3], b[9], b[18], c[9], c[3], c[18], c[8], c[12], c[19],
572 a[12], a[19], a[3], a[18], b[12], b[19], b[18], b[3], c[3], c[18], c[12],
573 c[19], a[19], a[18], b[19], b[18], c[18], c[19], u[1], a[12], a[19], a[10],
574 a[2], a[1], a[14], a[8], a[17], b[8], b[12], b[19], b[10], b[2], b[1],
575 b[14], b[17], c[14], c[17], c[2], c[8], c[12], c[1], c[10], c[19], a[19],
576 a[2], a[1], a[14], a[17], a[12], b[12], b[19], b[2], b[1], b[17], b[14],
577 c[14], c[17], c[1], c[12], c[19], c[2], a[12], a[19], a[3], a[18], a[13],
578 a[7], b[12], b[19], b[3], b[18], b[7], b[13], c[13], c[7], c[12], c[19],
579 c[3], c[18], a[19], a[18], a[7], b[19], b[18], b[7], c[7], c[19], c[18]
580 };
581 for (int i = 0; i < 1968; i++)
582 macaulay_matrix[indices[i]] = values[i];
583 }
584 #endif
585
586 // Transforms a 3 - vector in a 3x9 matrix such that :
587 // R * v = leftMultiplyMatrix(v) * vec(R)
588 // Where R is a rotation matrix and vec(R) converts R to a 9x1 vector.
589 Matx<double, 3, 9> leftMultiplyMatrix(const Vec3d& v) const {
590 Matx<double, 3, 9> left_mult_mat = Matx<double, 3, 9>::zeros();
591 left_mult_mat(0,0) = v[0]; left_mult_mat(0,1) = v[1]; left_mult_mat(0,2) = v[2];
592 left_mult_mat(1,3) = v[0]; left_mult_mat(1,4) = v[1]; left_mult_mat(1,5) = v[2];
593 left_mult_mat(2,6) = v[0]; left_mult_mat(2,7) = v[1]; left_mult_mat(2,8) = v[2];
594 return left_mult_mat;
595 }
596
597 // Extracts the coefficients of the Jacobians of the LS cost function (which is
598 // parameterized by the 3 rotation coefficients s1, s2, s3).
599 void extractJacobianCoefficients(const double * const D,
600 double f1_coeff[20], double f2_coeff[20], double f3_coeff[20]) const {
601 f1_coeff[0] =
602 2 * D[5] - 2 * D[7] + 2 * D[41] - 2 * D[43] + 2 * D[45] +
603 2 * D[49] + 2 * D[53] - 2 * D[63] - 2 * D[67] - 2 * D[71] +
604 2 * D[77] - 2 * D[79]; // constant term
605 f1_coeff[1] =
606 (6 * D[1] + 6 * D[3] + 6 * D[9] - 6 * D[13] - 6 * D[17] +
607 6 * D[27] - 6 * D[31] - 6 * D[35] - 6 * D[37] - 6 * D[39] -
608 6 * D[73] - 6 * D[75]); // s1^2 * s2
609 f1_coeff[2] =
610 (4 * D[6] - 4 * D[2] + 8 * D[14] - 8 * D[16] - 4 * D[18] +
611 4 * D[22] + 4 * D[26] + 8 * D[32] - 8 * D[34] + 4 * D[38] -
612 4 * D[42] + 8 * D[46] + 8 * D[48] + 4 * D[54] - 4 * D[58] -
613 4 * D[62] - 8 * D[64] - 8 * D[66] + 4 * D[74] -
614 4 * D[78]); // s1 * s2
615 f1_coeff[3] =
616 (4 * D[1] - 4 * D[3] + 4 * D[9] - 4 * D[13] - 4 * D[17] +
617 8 * D[23] - 8 * D[25] - 4 * D[27] + 4 * D[31] + 4 * D[35] -
618 4 * D[37] + 4 * D[39] + 8 * D[47] + 8 * D[51] + 8 * D[59] -
619 8 * D[61] - 8 * D[65] - 8 * D[69] - 4 * D[73] +
620 4 * D[75]); // s1 * s3
621 f1_coeff[4] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
622 8 * D[60] - 8 * D[70]); // s2 * s3
623 f1_coeff[5] =
624 (4 * D[14] - 2 * D[6] - 2 * D[2] + 4 * D[16] - 2 * D[18] +
625 2 * D[22] - 2 * D[26] + 4 * D[32] + 4 * D[34] + 2 * D[38] +
626 2 * D[42] + 4 * D[46] + 4 * D[48] - 2 * D[54] + 2 * D[58] -
627 2 * D[62] + 4 * D[64] + 4 * D[66] - 2 * D[74] -
628 2 * D[78]); // s2^2 * s3
629 f1_coeff[6] = (2 * D[13] - 2 * D[3] - 2 * D[9] - 2 * D[1] -
630 2 * D[17] - 2 * D[27] + 2 * D[31] - 2 * D[35] +
631 2 * D[37] + 2 * D[39] - 2 * D[73] - 2 * D[75]); // s2^3
632 f1_coeff[7] =
633 (4 * D[8] - 4 * D[0] + 8 * D[20] + 8 * D[24] + 4 * D[40] +
634 8 * D[56] + 8 * D[60] + 4 * D[72] - 4 * D[80]); // s1 * s3^2
635 f1_coeff[8] =
636 (4 * D[0] - 4 * D[40] - 4 * D[44] + 8 * D[50] - 8 * D[52] -
637 8 * D[68] + 8 * D[70] - 4 * D[76] - 4 * D[80]); // s1
638 f1_coeff[9] = (2 * D[2] + 2 * D[6] + 4 * D[14] - 4 * D[16] +
639 2 * D[18] + 2 * D[22] + 2 * D[26] - 4 * D[32] +
640 4 * D[34] + 2 * D[38] + 2 * D[42] + 4 * D[46] -
641 4 * D[48] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
642 4 * D[64] + 4 * D[66] + 2 * D[74] + 2 * D[78]); // s3
643 f1_coeff[10] = (2 * D[1] + 2 * D[3] + 2 * D[9] + 2 * D[13] +
644 2 * D[17] - 4 * D[23] + 4 * D[25] + 2 * D[27] +
645 2 * D[31] + 2 * D[35] + 2 * D[37] + 2 * D[39] -
646 4 * D[47] + 4 * D[51] + 4 * D[59] - 4 * D[61] +
647 4 * D[65] - 4 * D[69] + 2 * D[73] + 2 * D[75]); // s2
648 f1_coeff[11] =
649 (2 * D[17] - 2 * D[3] - 2 * D[9] - 2 * D[13] - 2 * D[1] +
650 4 * D[23] + 4 * D[25] - 2 * D[27] - 2 * D[31] + 2 * D[35] -
651 2 * D[37] - 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] +
652 4 * D[61] + 4 * D[65] + 4 * D[69] + 2 * D[73] +
653 2 * D[75]); // s2 * s3^2
654 f1_coeff[12] =
655 (6 * D[5] - 6 * D[7] - 6 * D[41] + 6 * D[43] + 6 * D[45] -
656 6 * D[49] - 6 * D[53] - 6 * D[63] + 6 * D[67] + 6 * D[71] -
657 6 * D[77] + 6 * D[79]); // s1^2
658 f1_coeff[13] =
659 (2 * D[7] - 2 * D[5] + 4 * D[11] + 4 * D[15] + 4 * D[19] -
660 4 * D[21] - 4 * D[29] - 4 * D[33] - 2 * D[41] + 2 * D[43] -
661 2 * D[45] - 2 * D[49] + 2 * D[53] + 4 * D[55] - 4 * D[57] +
662 2 * D[63] + 2 * D[67] - 2 * D[71] + 2 * D[77] -
663 2 * D[79]); // s3^2
664 f1_coeff[14] =
665 (2 * D[7] - 2 * D[5] - 4 * D[11] + 4 * D[15] - 4 * D[19] -
666 4 * D[21] - 4 * D[29] + 4 * D[33] + 2 * D[41] - 2 * D[43] -
667 2 * D[45] + 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
668 2 * D[63] - 2 * D[67] + 2 * D[71] - 2 * D[77] +
669 2 * D[79]); // s2^2
670 f1_coeff[15] =
671 (2 * D[26] - 2 * D[6] - 2 * D[18] - 2 * D[22] - 2 * D[2] -
672 2 * D[38] - 2 * D[42] - 2 * D[54] - 2 * D[58] + 2 * D[62] +
673 2 * D[74] + 2 * D[78]); // s3^3
674 f1_coeff[16] =
675 (4 * D[5] + 4 * D[7] + 8 * D[11] + 8 * D[15] + 8 * D[19] +
676 8 * D[21] + 8 * D[29] + 8 * D[33] - 4 * D[41] - 4 * D[43] +
677 4 * D[45] - 4 * D[49] - 4 * D[53] + 8 * D[55] + 8 * D[57] +
678 4 * D[63] - 4 * D[67] - 4 * D[71] - 4 * D[77] -
679 4 * D[79]); // s1 * s2 * s3
680 f1_coeff[17] =
681 (4 * D[4] - 4 * D[0] + 8 * D[10] + 8 * D[12] + 8 * D[28] +
682 8 * D[30] + 4 * D[36] - 4 * D[40] + 4 * D[80]); // s1 * s2^2
683 f1_coeff[18] =
684 (6 * D[2] + 6 * D[6] + 6 * D[18] - 6 * D[22] - 6 * D[26] -
685 6 * D[38] - 6 * D[42] + 6 * D[54] - 6 * D[58] - 6 * D[62] -
686 6 * D[74] - 6 * D[78]); // s1^2 * s3
687 f1_coeff[19] =
688 (4 * D[0] - 4 * D[4] - 4 * D[8] - 4 * D[36] + 4 * D[40] +
689 4 * D[44] - 4 * D[72] + 4 * D[76] + 4 * D[80]); // s1^3
690
691 f2_coeff[0] =
692 -2 * D[2] + 2 * D[6] - 2 * D[18] - 2 * D[22] - 2 * D[26] -
693 2 * D[38] + 2 * D[42] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
694 2 * D[74] + 2 * D[78]; // constant term
695 f2_coeff[1] =
696 (4 * D[4] - 4 * D[0] + 8 * D[10] + 8 * D[12] + 8 * D[28] +
697 8 * D[30] + 4 * D[36] - 4 * D[40] + 4 * D[80]); // s1^2 * s2
698 f2_coeff[2] =
699 (4 * D[7] - 4 * D[5] - 8 * D[11] + 8 * D[15] - 8 * D[19] -
700 8 * D[21] - 8 * D[29] + 8 * D[33] + 4 * D[41] - 4 * D[43] -
701 4 * D[45] + 4 * D[49] - 4 * D[53] + 8 * D[55] + 8 * D[57] +
702 4 * D[63] - 4 * D[67] + 4 * D[71] - 4 * D[77] +
703 4 * D[79]); // s1 * s2
704 f2_coeff[3] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
705 8 * D[60] - 8 * D[70]); // s1 * s3
706 f2_coeff[4] =
707 (4 * D[3] - 4 * D[1] - 4 * D[9] + 4 * D[13] - 4 * D[17] -
708 8 * D[23] - 8 * D[25] + 4 * D[27] - 4 * D[31] + 4 * D[35] +
709 4 * D[37] - 4 * D[39] - 8 * D[47] + 8 * D[51] + 8 * D[59] +
710 8 * D[61] - 8 * D[65] + 8 * D[69] - 4 * D[73] +
711 4 * D[75]); // s2 * s3
712 f2_coeff[5] =
713 (6 * D[41] - 6 * D[7] - 6 * D[5] + 6 * D[43] - 6 * D[45] +
714 6 * D[49] - 6 * D[53] - 6 * D[63] + 6 * D[67] - 6 * D[71] -
715 6 * D[77] - 6 * D[79]); // s2^2 * s3
716 f2_coeff[6] =
717 (4 * D[0] - 4 * D[4] + 4 * D[8] - 4 * D[36] + 4 * D[40] -
718 4 * D[44] + 4 * D[72] - 4 * D[76] + 4 * D[80]); // s2^3
719 f2_coeff[7] =
720 (2 * D[17] - 2 * D[3] - 2 * D[9] - 2 * D[13] - 2 * D[1] +
721 4 * D[23] + 4 * D[25] - 2 * D[27] - 2 * D[31] + 2 * D[35] -
722 2 * D[37] - 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] +
723 4 * D[61] + 4 * D[65] + 4 * D[69] + 2 * D[73] +
724 2 * D[75]); // s1 * s3^2
725 f2_coeff[8] = (2 * D[1] + 2 * D[3] + 2 * D[9] + 2 * D[13] +
726 2 * D[17] - 4 * D[23] + 4 * D[25] + 2 * D[27] +
727 2 * D[31] + 2 * D[35] + 2 * D[37] + 2 * D[39] -
728 4 * D[47] + 4 * D[51] + 4 * D[59] - 4 * D[61] +
729 4 * D[65] - 4 * D[69] + 2 * D[73] + 2 * D[75]); // s1
730 f2_coeff[9] = (2 * D[5] + 2 * D[7] - 4 * D[11] + 4 * D[15] -
731 4 * D[19] + 4 * D[21] + 4 * D[29] - 4 * D[33] +
732 2 * D[41] + 2 * D[43] + 2 * D[45] + 2 * D[49] +
733 2 * D[53] + 4 * D[55] - 4 * D[57] + 2 * D[63] +
734 2 * D[67] + 2 * D[71] + 2 * D[77] + 2 * D[79]); // s3
735 f2_coeff[10] =
736 (8 * D[20] - 4 * D[8] - 4 * D[0] - 8 * D[24] + 4 * D[40] -
737 8 * D[56] + 8 * D[60] - 4 * D[72] - 4 * D[80]); // s2
738 f2_coeff[11] =
739 (4 * D[0] - 4 * D[40] + 4 * D[44] + 8 * D[50] + 8 * D[52] +
740 8 * D[68] + 8 * D[70] + 4 * D[76] - 4 * D[80]); // s2 * s3^2
741 f2_coeff[12] =
742 (2 * D[6] - 2 * D[2] + 4 * D[14] - 4 * D[16] - 2 * D[18] +
743 2 * D[22] + 2 * D[26] + 4 * D[32] - 4 * D[34] + 2 * D[38] -
744 2 * D[42] + 4 * D[46] + 4 * D[48] + 2 * D[54] - 2 * D[58] -
745 2 * D[62] - 4 * D[64] - 4 * D[66] + 2 * D[74] -
746 2 * D[78]); // s1^2
747 f2_coeff[13] =
748 (2 * D[2] - 2 * D[6] + 4 * D[14] + 4 * D[16] + 2 * D[18] +
749 2 * D[22] - 2 * D[26] - 4 * D[32] - 4 * D[34] + 2 * D[38] -
750 2 * D[42] + 4 * D[46] - 4 * D[48] - 2 * D[54] - 2 * D[58] +
751 2 * D[62] + 4 * D[64] - 4 * D[66] - 2 * D[74] +
752 2 * D[78]); // s3^2
753 f2_coeff[14] =
754 (6 * D[2] - 6 * D[6] + 6 * D[18] - 6 * D[22] + 6 * D[26] -
755 6 * D[38] + 6 * D[42] - 6 * D[54] + 6 * D[58] - 6 * D[62] +
756 6 * D[74] - 6 * D[78]); // s2^2
757 f2_coeff[15] =
758 (2 * D[53] - 2 * D[7] - 2 * D[41] - 2 * D[43] - 2 * D[45] -
759 2 * D[49] - 2 * D[5] - 2 * D[63] - 2 * D[67] + 2 * D[71] +
760 2 * D[77] + 2 * D[79]); // s3^3
761 f2_coeff[16] =
762 (8 * D[14] - 4 * D[6] - 4 * D[2] + 8 * D[16] - 4 * D[18] +
763 4 * D[22] - 4 * D[26] + 8 * D[32] + 8 * D[34] + 4 * D[38] +
764 4 * D[42] + 8 * D[46] + 8 * D[48] - 4 * D[54] + 4 * D[58] -
765 4 * D[62] + 8 * D[64] + 8 * D[66] - 4 * D[74] -
766 4 * D[78]); // s1 * s2 * s3
767 f2_coeff[17] =
768 (6 * D[13] - 6 * D[3] - 6 * D[9] - 6 * D[1] - 6 * D[17] -
769 6 * D[27] + 6 * D[31] - 6 * D[35] + 6 * D[37] + 6 * D[39] -
770 6 * D[73] - 6 * D[75]); // s1 * s2^2
771 f2_coeff[18] =
772 (2 * D[5] + 2 * D[7] + 4 * D[11] + 4 * D[15] + 4 * D[19] +
773 4 * D[21] + 4 * D[29] + 4 * D[33] - 2 * D[41] - 2 * D[43] +
774 2 * D[45] - 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
775 2 * D[63] - 2 * D[67] - 2 * D[71] - 2 * D[77] -
776 2 * D[79]); // s1^2 * s3
777 f2_coeff[19] =
778 (2 * D[1] + 2 * D[3] + 2 * D[9] - 2 * D[13] - 2 * D[17] +
779 2 * D[27] - 2 * D[31] - 2 * D[35] - 2 * D[37] - 2 * D[39] -
780 2 * D[73] - 2 * D[75]); // s1^3
781
782 f3_coeff[0] =
783 2 * D[1] - 2 * D[3] + 2 * D[9] + 2 * D[13] + 2 * D[17] -
784 2 * D[27] - 2 * D[31] - 2 * D[35] + 2 * D[37] - 2 * D[39] +
785 2 * D[73] - 2 * D[75]; // constant term
786 f3_coeff[1] =
787 (2 * D[5] + 2 * D[7] + 4 * D[11] + 4 * D[15] + 4 * D[19] +
788 4 * D[21] + 4 * D[29] + 4 * D[33] - 2 * D[41] - 2 * D[43] +
789 2 * D[45] - 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
790 2 * D[63] - 2 * D[67] - 2 * D[71] - 2 * D[77] -
791 2 * D[79]); // s1^2 * s2
792 f3_coeff[2] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
793 8 * D[60] - 8 * D[70]); // s1 * s2
794 f3_coeff[3] =
795 (4 * D[7] - 4 * D[5] + 8 * D[11] + 8 * D[15] + 8 * D[19] -
796 8 * D[21] - 8 * D[29] - 8 * D[33] - 4 * D[41] + 4 * D[43] -
797 4 * D[45] - 4 * D[49] + 4 * D[53] + 8 * D[55] - 8 * D[57] +
798 4 * D[63] + 4 * D[67] - 4 * D[71] + 4 * D[77] -
799 4 * D[79]); // s1 * s3
800 f3_coeff[4] =
801 (4 * D[2] - 4 * D[6] + 8 * D[14] + 8 * D[16] + 4 * D[18] +
802 4 * D[22] - 4 * D[26] - 8 * D[32] - 8 * D[34] + 4 * D[38] -
803 4 * D[42] + 8 * D[46] - 8 * D[48] - 4 * D[54] - 4 * D[58] +
804 4 * D[62] + 8 * D[64] - 8 * D[66] - 4 * D[74] +
805 4 * D[78]); // s2 * s3
806 f3_coeff[5] =
807 (4 * D[0] - 4 * D[40] + 4 * D[44] + 8 * D[50] + 8 * D[52] +
808 8 * D[68] + 8 * D[70] + 4 * D[76] - 4 * D[80]); // s2^2 * s3
809 f3_coeff[6] = (2 * D[41] - 2 * D[7] - 2 * D[5] + 2 * D[43] -
810 2 * D[45] + 2 * D[49] - 2 * D[53] - 2 * D[63] +
811 2 * D[67] - 2 * D[71] - 2 * D[77] - 2 * D[79]); // s2^3
812 f3_coeff[7] =
813 (6 * D[26] - 6 * D[6] - 6 * D[18] - 6 * D[22] - 6 * D[2] -
814 6 * D[38] - 6 * D[42] - 6 * D[54] - 6 * D[58] + 6 * D[62] +
815 6 * D[74] + 6 * D[78]); // s1 * s3^2
816 f3_coeff[8] = (2 * D[2] + 2 * D[6] + 4 * D[14] - 4 * D[16] +
817 2 * D[18] + 2 * D[22] + 2 * D[26] - 4 * D[32] +
818 4 * D[34] + 2 * D[38] + 2 * D[42] + 4 * D[46] -
819 4 * D[48] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
820 4 * D[64] + 4 * D[66] + 2 * D[74] + 2 * D[78]); // s1
821 f3_coeff[9] =
822 (8 * D[10] - 4 * D[4] - 4 * D[0] - 8 * D[12] - 8 * D[28] +
823 8 * D[30] - 4 * D[36] - 4 * D[40] + 4 * D[80]); // s3
824 f3_coeff[10] = (2 * D[5] + 2 * D[7] - 4 * D[11] + 4 * D[15] -
825 4 * D[19] + 4 * D[21] + 4 * D[29] - 4 * D[33] +
826 2 * D[41] + 2 * D[43] + 2 * D[45] + 2 * D[49] +
827 2 * D[53] + 4 * D[55] - 4 * D[57] + 2 * D[63] +
828 2 * D[67] + 2 * D[71] + 2 * D[77] + 2 * D[79]); // s2
829 f3_coeff[11] =
830 (6 * D[53] - 6 * D[7] - 6 * D[41] - 6 * D[43] - 6 * D[45] -
831 6 * D[49] - 6 * D[5] - 6 * D[63] - 6 * D[67] + 6 * D[71] +
832 6 * D[77] + 6 * D[79]); // s2 * s3^2
833 f3_coeff[12] =
834 (2 * D[1] - 2 * D[3] + 2 * D[9] - 2 * D[13] - 2 * D[17] +
835 4 * D[23] - 4 * D[25] - 2 * D[27] + 2 * D[31] + 2 * D[35] -
836 2 * D[37] + 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] -
837 4 * D[61] - 4 * D[65] - 4 * D[69] - 2 * D[73] +
838 2 * D[75]); // s1^2
839 f3_coeff[13] =
840 (6 * D[3] - 6 * D[1] - 6 * D[9] - 6 * D[13] + 6 * D[17] +
841 6 * D[27] + 6 * D[31] - 6 * D[35] - 6 * D[37] + 6 * D[39] +
842 6 * D[73] - 6 * D[75]); // s3^2
843 f3_coeff[14] =
844 (2 * D[3] - 2 * D[1] - 2 * D[9] + 2 * D[13] - 2 * D[17] -
845 4 * D[23] - 4 * D[25] + 2 * D[27] - 2 * D[31] + 2 * D[35] +
846 2 * D[37] - 2 * D[39] - 4 * D[47] + 4 * D[51] + 4 * D[59] +
847 4 * D[61] - 4 * D[65] + 4 * D[69] - 2 * D[73] +
848 2 * D[75]); // s2^2
849 f3_coeff[15] =
850 (4 * D[0] + 4 * D[4] - 4 * D[8] + 4 * D[36] + 4 * D[40] -
851 4 * D[44] - 4 * D[72] - 4 * D[76] + 4 * D[80]); // s3^3
852 f3_coeff[16] =
853 (4 * D[17] - 4 * D[3] - 4 * D[9] - 4 * D[13] - 4 * D[1] +
854 8 * D[23] + 8 * D[25] - 4 * D[27] - 4 * D[31] + 4 * D[35] -
855 4 * D[37] - 4 * D[39] + 8 * D[47] + 8 * D[51] + 8 * D[59] +
856 8 * D[61] + 8 * D[65] + 8 * D[69] + 4 * D[73] +
857 4 * D[75]); // s1 * s2 * s3
858 f3_coeff[17] =
859 (4 * D[14] - 2 * D[6] - 2 * D[2] + 4 * D[16] - 2 * D[18] +
860 2 * D[22] - 2 * D[26] + 4 * D[32] + 4 * D[34] + 2 * D[38] +
861 2 * D[42] + 4 * D[46] + 4 * D[48] - 2 * D[54] + 2 * D[58] -
862 2 * D[62] + 4 * D[64] + 4 * D[66] - 2 * D[74] -
863 2 * D[78]); // s1 * s2^2
864 f3_coeff[18] =
865 (4 * D[8] - 4 * D[0] + 8 * D[20] + 8 * D[24] + 4 * D[40] +
866 8 * D[56] + 8 * D[60] + 4 * D[72] - 4 * D[80]); // s1^2 * s3
867 f3_coeff[19] =
868 (2 * D[2] + 2 * D[6] + 2 * D[18] - 2 * D[22] - 2 * D[26] -
869 2 * D[38] - 2 * D[42] + 2 * D[54] - 2 * D[58] - 2 * D[62] -
870 2 * D[74] - 2 * D[78]); // s1^3
871 }
872 };
create(const Mat & points_,const Mat & calib_norm_pts,const Mat & K)873 Ptr<DLSPnP> DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
874 return makePtr<DLSPnPImpl>(points_, calib_norm_pts, K);
875 }
876 }}
877