1 /* Copyright (C) 2013-2016, The Regents of The University of Michigan.
2 All rights reserved.
3 This software was developed in the APRIL Robotics Lab under the
4 direction of Edwin Olson, ebolson@umich.edu. This software may be
5 available under alternative licensing terms; contact the address above.
6 Redistribution and use in source and binary forms, with or without
7 modification, are permitted provided that the following conditions are met:
8 1. Redistributions of source code must retain the above copyright notice, this
9 list of conditions and the following disclaimer.
10 2. Redistributions in binary form must reproduce the above copyright notice,
11 this list of conditions and the following disclaimer in the documentation
12 and/or other materials provided with the distribution.
13 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
16 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
17 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
20 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
21 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
22 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 The views and conclusions contained in the software and documentation are those
24 of the authors and should not be interpreted as representing official policies,
25 either expressed or implied, of the Regents of The University of Michigan.
26 */
27
28 #include <math.h>
29 #include <stdio.h>
30
31 #include "common/matd.h"
32 #include "common/zarray.h"
33 #include "common/homography.h"
34 #include "common/math_util.h"
35
36 // correspondences is a list of float[4]s, consisting of the points x
37 // and y concatenated. We will compute a homography such that y = Hx
homography_compute(zarray_t * correspondences,int flags)38 matd_t *homography_compute(zarray_t *correspondences, int flags)
39 {
40 // compute centroids of both sets of points (yields a better
41 // conditioned information matrix)
42 double x_cx = 0, x_cy = 0;
43 double y_cx = 0, y_cy = 0;
44
45 for (int i = 0; i < zarray_size(correspondences); i++) {
46 float *c;
47 zarray_get_volatile(correspondences, i, &c);
48
49 x_cx += c[0];
50 x_cy += c[1];
51 y_cx += c[2];
52 y_cy += c[3];
53 }
54
55 int sz = zarray_size(correspondences);
56 x_cx /= sz;
57 x_cy /= sz;
58 y_cx /= sz;
59 y_cy /= sz;
60
61 // NB We don't normalize scale; it seems implausible that it could
62 // possibly make any difference given the dynamic range of IEEE
63 // doubles.
64
65 matd_t *A = matd_create(9,9);
66 for (int i = 0; i < zarray_size(correspondences); i++) {
67 float *c;
68 zarray_get_volatile(correspondences, i, &c);
69
70 // (below world is "x", and image is "y")
71 double worldx = c[0] - x_cx;
72 double worldy = c[1] - x_cy;
73 double imagex = c[2] - y_cx;
74 double imagey = c[3] - y_cy;
75
76 double a03 = -worldx;
77 double a04 = -worldy;
78 double a05 = -1;
79 double a06 = worldx*imagey;
80 double a07 = worldy*imagey;
81 double a08 = imagey;
82
83 MATD_EL(A, 3, 3) += a03*a03;
84 MATD_EL(A, 3, 4) += a03*a04;
85 MATD_EL(A, 3, 5) += a03*a05;
86 MATD_EL(A, 3, 6) += a03*a06;
87 MATD_EL(A, 3, 7) += a03*a07;
88 MATD_EL(A, 3, 8) += a03*a08;
89 MATD_EL(A, 4, 4) += a04*a04;
90 MATD_EL(A, 4, 5) += a04*a05;
91 MATD_EL(A, 4, 6) += a04*a06;
92 MATD_EL(A, 4, 7) += a04*a07;
93 MATD_EL(A, 4, 8) += a04*a08;
94 MATD_EL(A, 5, 5) += a05*a05;
95 MATD_EL(A, 5, 6) += a05*a06;
96 MATD_EL(A, 5, 7) += a05*a07;
97 MATD_EL(A, 5, 8) += a05*a08;
98 MATD_EL(A, 6, 6) += a06*a06;
99 MATD_EL(A, 6, 7) += a06*a07;
100 MATD_EL(A, 6, 8) += a06*a08;
101 MATD_EL(A, 7, 7) += a07*a07;
102 MATD_EL(A, 7, 8) += a07*a08;
103 MATD_EL(A, 8, 8) += a08*a08;
104
105 double a10 = worldx;
106 double a11 = worldy;
107 double a12 = 1;
108 double a16 = -worldx*imagex;
109 double a17 = -worldy*imagex;
110 double a18 = -imagex;
111
112 MATD_EL(A, 0, 0) += a10*a10;
113 MATD_EL(A, 0, 1) += a10*a11;
114 MATD_EL(A, 0, 2) += a10*a12;
115 MATD_EL(A, 0, 6) += a10*a16;
116 MATD_EL(A, 0, 7) += a10*a17;
117 MATD_EL(A, 0, 8) += a10*a18;
118 MATD_EL(A, 1, 1) += a11*a11;
119 MATD_EL(A, 1, 2) += a11*a12;
120 MATD_EL(A, 1, 6) += a11*a16;
121 MATD_EL(A, 1, 7) += a11*a17;
122 MATD_EL(A, 1, 8) += a11*a18;
123 MATD_EL(A, 2, 2) += a12*a12;
124 MATD_EL(A, 2, 6) += a12*a16;
125 MATD_EL(A, 2, 7) += a12*a17;
126 MATD_EL(A, 2, 8) += a12*a18;
127 MATD_EL(A, 6, 6) += a16*a16;
128 MATD_EL(A, 6, 7) += a16*a17;
129 MATD_EL(A, 6, 8) += a16*a18;
130 MATD_EL(A, 7, 7) += a17*a17;
131 MATD_EL(A, 7, 8) += a17*a18;
132 MATD_EL(A, 8, 8) += a18*a18;
133
134 double a20 = -worldx*imagey;
135 double a21 = -worldy*imagey;
136 double a22 = -imagey;
137 double a23 = worldx*imagex;
138 double a24 = worldy*imagex;
139 double a25 = imagex;
140
141 MATD_EL(A, 0, 0) += a20*a20;
142 MATD_EL(A, 0, 1) += a20*a21;
143 MATD_EL(A, 0, 2) += a20*a22;
144 MATD_EL(A, 0, 3) += a20*a23;
145 MATD_EL(A, 0, 4) += a20*a24;
146 MATD_EL(A, 0, 5) += a20*a25;
147 MATD_EL(A, 1, 1) += a21*a21;
148 MATD_EL(A, 1, 2) += a21*a22;
149 MATD_EL(A, 1, 3) += a21*a23;
150 MATD_EL(A, 1, 4) += a21*a24;
151 MATD_EL(A, 1, 5) += a21*a25;
152 MATD_EL(A, 2, 2) += a22*a22;
153 MATD_EL(A, 2, 3) += a22*a23;
154 MATD_EL(A, 2, 4) += a22*a24;
155 MATD_EL(A, 2, 5) += a22*a25;
156 MATD_EL(A, 3, 3) += a23*a23;
157 MATD_EL(A, 3, 4) += a23*a24;
158 MATD_EL(A, 3, 5) += a23*a25;
159 MATD_EL(A, 4, 4) += a24*a24;
160 MATD_EL(A, 4, 5) += a24*a25;
161 MATD_EL(A, 5, 5) += a25*a25;
162 }
163
164 // make symmetric
165 for (int i = 0; i < 9; i++)
166 for (int j = i+1; j < 9; j++)
167 MATD_EL(A, j, i) = MATD_EL(A, i, j);
168
169 matd_t *H = matd_create(3,3);
170
171 if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
172 // compute singular vector by (carefully) inverting the rank-deficient matrix.
173
174 if (1) {
175 matd_t *Ainv = matd_inverse(A);
176 double scale = 0;
177
178 for (int i = 0; i < 9; i++)
179 scale += sq(MATD_EL(Ainv, i, 0));
180 scale = sqrt(scale);
181
182 for (int i = 0; i < 3; i++)
183 for (int j = 0; j < 3; j++)
184 MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
185
186 matd_destroy(Ainv);
187 } else {
188 double data_b[] = {1, 0, 0, 0, 0, 0, 0, 0, 0};
189 matd_t *b = matd_create_data(9, 1, data_b);
190 matd_t *Ainv = NULL;
191
192 if (0) {
193 matd_plu_t *lu = matd_plu(A);
194 Ainv = matd_plu_solve(lu, b);
195 matd_plu_destroy(lu);
196 } else {
197 matd_chol_t *chol = matd_chol(A);
198 Ainv = matd_chol_solve(chol, b);
199 matd_chol_destroy(chol);
200 }
201
202 double scale = 0;
203
204 for (int i = 0; i < 9; i++)
205 scale += sq(MATD_EL(Ainv, i, 0));
206 scale = sqrt(scale);
207
208 for (int i = 0; i < 3; i++)
209 for (int j = 0; j < 3; j++)
210 MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
211
212 matd_destroy(b);
213 matd_destroy(Ainv);
214 }
215
216 } else {
217 // compute singular vector using SVD. A bit slower, but more accurate.
218 matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS);
219
220 for (int i = 0; i < 3; i++)
221 for (int j = 0; j < 3; j++)
222 MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
223
224 matd_destroy(svd.U);
225 matd_destroy(svd.S);
226 matd_destroy(svd.V);
227
228 }
229
230 matd_t *Tx = matd_identity(3);
231 MATD_EL(Tx,0,2) = -x_cx;
232 MATD_EL(Tx,1,2) = -x_cy;
233
234 matd_t *Ty = matd_identity(3);
235 MATD_EL(Ty,0,2) = y_cx;
236 MATD_EL(Ty,1,2) = y_cy;
237
238 matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
239
240 matd_destroy(A);
241 matd_destroy(Tx);
242 matd_destroy(Ty);
243 matd_destroy(H);
244
245 return H2;
246 }
247
248
249 // assuming that the projection matrix is:
250 // [ fx 0 cx 0 ]
251 // [ 0 fy cy 0 ]
252 // [ 0 0 1 0 ]
253 //
254 // And that the homography is equal to the projection matrix times the
255 // model matrix, recover the model matrix (which is returned). Note
256 // that the third column of the model matrix is missing in the
257 // expresison below, reflecting the fact that the homography assumes
258 // all points are at z=0 (i.e., planar) and that the element of z is
259 // thus omitted. (3x1 instead of 4x1).
260 //
261 // [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
262 // [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
263 // [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
264 // [ 0 0 1 ]
265 //
266 // fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
267 // fx*R01 + cx*R21 = H01
268 // fx*TX + cx*TZ = H02
269 // fy*R10 + cy*R20 = H10
270 // fy*R11 + cy*R21 = H11
271 // fy*TY + cy*TZ = H12
272 // R20 = H20
273 // R21 = H21
274 // TZ = H22
275
homography_to_pose(const matd_t * H,double fx,double fy,double cx,double cy)276 matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy)
277 {
278 // Note that every variable that we compute is proportional to the scale factor of H.
279 double R20 = MATD_EL(H, 2, 0);
280 double R21 = MATD_EL(H, 2, 1);
281 double TZ = MATD_EL(H, 2, 2);
282 double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx;
283 double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx;
284 double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx;
285 double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy;
286 double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy;
287 double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy;
288
289 // compute the scale by requiring that the rotation columns are unit length
290 // (Use geometric average of the two length vectors we have)
291 double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
292 double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
293 double s = 1.0 / sqrtf(length1 * length2);
294
295 // get sign of S by requiring the tag to be in front the camera;
296 // we assume camera looks in the -Z direction.
297 if (TZ > 0)
298 s *= -1;
299
300 R20 *= s;
301 R21 *= s;
302 TZ *= s;
303 R00 *= s;
304 R01 *= s;
305 TX *= s;
306 R10 *= s;
307 R11 *= s;
308 TY *= s;
309
310 // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
311 double R02 = R10*R21 - R20*R11;
312 double R12 = R20*R01 - R00*R21;
313 double R22 = R00*R11 - R10*R01;
314
315 // Improve rotation matrix by applying polar decomposition.
316 if (1) {
317 // do polar decomposition. This makes the rotation matrix
318 // "proper", but probably increases the reprojection error. An
319 // iterative alignment step would be superior.
320
321 double data_[] = { R00, R01, R02,
322 R10, R11, R12,
323 R20, R21, R22 };
324 matd_t *R = matd_create_data(3, 3, data_);
325
326 matd_svd_t svd = matd_svd(R);
327 matd_destroy(R);
328
329 R = matd_op("M*M'", svd.U, svd.V);
330
331 matd_destroy(svd.U);
332 matd_destroy(svd.S);
333 matd_destroy(svd.V);
334
335 R00 = MATD_EL(R, 0, 0);
336 R01 = MATD_EL(R, 0, 1);
337 R02 = MATD_EL(R, 0, 2);
338 R10 = MATD_EL(R, 1, 0);
339 R11 = MATD_EL(R, 1, 1);
340 R12 = MATD_EL(R, 1, 2);
341 R20 = MATD_EL(R, 2, 0);
342 R21 = MATD_EL(R, 2, 1);
343 R22 = MATD_EL(R, 2, 2);
344
345 matd_destroy(R);
346 }
347
348 double data_[] = { R00, R01, R02, TX,
349 R10, R11, R12, TY,
350 R20, R21, R22, TZ,
351 0, 0, 0, 1 };
352 return matd_create_data(4, 4, data_);
353 }
354
355 // Similar to above
356 // Recover the model view matrix assuming that the projection matrix is:
357 //
358 // [ F 0 A 0 ] (see glFrustrum)
359 // [ 0 G B 0 ]
360 // [ 0 0 C D ]
361 // [ 0 0 -1 0 ]
362
homography_to_model_view(const matd_t * H,double F,double G,double A,double B,double C,double D)363 matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D)
364 {
365 // Note that every variable that we compute is proportional to the scale factor of H.
366 double R20 = -MATD_EL(H, 2, 0);
367 double R21 = -MATD_EL(H, 2, 1);
368 double TZ = -MATD_EL(H, 2, 2);
369 double R00 = (MATD_EL(H, 0, 0) - A*R20) / F;
370 double R01 = (MATD_EL(H, 0, 1) - A*R21) / F;
371 double TX = (MATD_EL(H, 0, 2) - A*TZ) / F;
372 double R10 = (MATD_EL(H, 1, 0) - B*R20) / G;
373 double R11 = (MATD_EL(H, 1, 1) - B*R21) / G;
374 double TY = (MATD_EL(H, 1, 2) - B*TZ) / G;
375
376 // compute the scale by requiring that the rotation columns are unit length
377 // (Use geometric average of the two length vectors we have)
378 double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
379 double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
380 double s = 1.0 / sqrtf(length1 * length2);
381
382 // get sign of S by requiring the tag to be in front of the camera
383 // (which is Z < 0) for our conventions.
384 if (TZ > 0)
385 s *= -1;
386
387 R20 *= s;
388 R21 *= s;
389 TZ *= s;
390 R00 *= s;
391 R01 *= s;
392 TX *= s;
393 R10 *= s;
394 R11 *= s;
395 TY *= s;
396
397 // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
398 double R02 = R10*R21 - R20*R11;
399 double R12 = R20*R01 - R00*R21;
400 double R22 = R00*R11 - R10*R01;
401
402 // TODO XXX: Improve rotation matrix by applying polar decomposition.
403 double data_[] = { R00, R01, R02, TX,
404 R10, R11, R12, TY,
405 R20, R21, R22, TZ,
406 0, 0, 0, 1 };
407 return matd_create_data(4, 4, data_);
408 }
409
410 // Only uses the upper 3x3 matrix.
411 /*
412 static void matrix_to_quat(const matd_t *R, double q[4])
413 {
414 // see: "from quaternion to matrix and back"
415
416 // trace: get the same result if R is 4x4 or 3x3:
417 double T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1;
418 double S = 0;
419
420 double m0 = MATD_EL(R, 0, 0);
421 double m1 = MATD_EL(R, 1, 0);
422 double m2 = MATD_EL(R, 2, 0);
423 double m4 = MATD_EL(R, 0, 1);
424 double m5 = MATD_EL(R, 1, 1);
425 double m6 = MATD_EL(R, 2, 1);
426 double m8 = MATD_EL(R, 0, 2);
427 double m9 = MATD_EL(R, 1, 2);
428 double m10 = MATD_EL(R, 2, 2);
429
430 if (T > 0.0000001) {
431 S = sqrtf(T) * 2;
432 q[1] = -( m9 - m6 ) / S;
433 q[2] = -( m2 - m8 ) / S;
434 q[3] = -( m4 - m1 ) / S;
435 q[0] = 0.25 * S;
436 } else if ( m0 > m5 && m0 > m10 ) { // Column 0:
437 S = sqrtf( 1.0 + m0 - m5 - m10 ) * 2;
438 q[1] = -0.25 * S;
439 q[2] = -(m4 + m1 ) / S;
440 q[3] = -(m2 + m8 ) / S;
441 q[0] = (m9 - m6 ) / S;
442 } else if ( m5 > m10 ) { // Column 1:
443 S = sqrtf( 1.0 + m5 - m0 - m10 ) * 2;
444 q[1] = -(m4 + m1 ) / S;
445 q[2] = -0.25 * S;
446 q[3] = -(m9 + m6 ) / S;
447 q[0] = (m2 - m8 ) / S;
448 } else {
449 // Column 2:
450 S = sqrtf( 1.0 + m10 - m0 - m5 ) * 2;
451 q[1] = -(m2 + m8 ) / S;
452 q[2] = -(m9 + m6 ) / S;
453 q[3] = -0.25 * S;
454 q[0] = (m4 - m1 ) / S;
455 }
456
457 double mag2 = 0;
458 for (int i = 0; i < 4; i++)
459 mag2 += q[i]*q[i];
460 double norm = 1.0 / sqrtf(mag2);
461 for (int i = 0; i < 4; i++)
462 q[i] *= norm;
463 }
464 */
465
466 // overwrites upper 3x3 area of matrix M. Doesn't touch any other elements of M.
quat_to_matrix(const double q[4],matd_t * M)467 void quat_to_matrix(const double q[4], matd_t *M)
468 {
469 double w = q[0], x = q[1], y = q[2], z = q[3];
470
471 MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z;
472 MATD_EL(M, 0, 1) = 2*x*y - 2*w*z;
473 MATD_EL(M, 0, 2) = 2*x*z + 2*w*y;
474
475 MATD_EL(M, 1, 0) = 2*x*y + 2*w*z;
476 MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z;
477 MATD_EL(M, 1, 2) = 2*y*z - 2*w*x;
478
479 MATD_EL(M, 2, 0) = 2*x*z - 2*w*y;
480 MATD_EL(M, 2, 1) = 2*y*z + 2*w*x;
481 MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z;
482 }
483