Fix T43905: Crash happens when press Create Plane Track
[blender.git] / extern / libmv / libmv / multiview / homography.cc
1 // Copyright (c) 2008, 2009 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/multiview/homography.h"
22
23 #include "ceres/ceres.h"
24 #include "libmv/logging/logging.h"
25 #include "libmv/multiview/conditioning.h"
26 #include "libmv/multiview/homography_parameterization.h"
27
28 namespace libmv {
29 /** 2D Homography transformation estimation in the case that points are in 
30  * euclidean coordinates.
31  *
32  * x = H y
33  * x and y vector must have the same direction, we could write
34  * crossproduct(|x|, * H * |y| ) = |0|
35  *
36  * | 0 -1  x2|   |a b c|   |y1|    |0|
37  * | 1  0 -x1| * |d e f| * |y2| =  |0|
38  * |-x2  x1 0|   |g h 1|   |1 |    |0|
39  *
40  * That gives :
41  *
42  * (-d+x2*g)*y1    + (-e+x2*h)*y2 + -f+x2          |0|
43  * (a-x1*g)*y1     + (b-x1*h)*y2  + c-x1         = |0|
44  * (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f  |0|
45  */
46 static bool Homography2DFromCorrespondencesLinearEuc(
47     const Mat &x1,
48     const Mat &x2,
49     Mat3 *H,
50     double expected_precision) {
51   assert(2 == x1.rows());
52   assert(4 <= x1.cols());
53   assert(x1.rows() == x2.rows());
54   assert(x1.cols() == x2.cols());
55
56   int n = x1.cols();
57   MatX8 L = Mat::Zero(n * 3, 8);
58   Mat b = Mat::Zero(n * 3, 1);
59   for (int i = 0; i < n; ++i) {
60     int j = 3 * i;
61     L(j, 0) =  x1(0, i);             // a
62     L(j, 1) =  x1(1, i);             // b
63     L(j, 2) =  1.0;                  // c
64     L(j, 6) = -x2(0, i) * x1(0, i);  // g
65     L(j, 7) = -x2(0, i) * x1(1, i);  // h
66     b(j, 0) =  x2(0, i);             // i
67
68     ++j;
69     L(j, 3) =  x1(0, i);             // d
70     L(j, 4) =  x1(1, i);             // e
71     L(j, 5) =  1.0;                  // f
72     L(j, 6) = -x2(1, i) * x1(0, i);  // g
73     L(j, 7) = -x2(1, i) * x1(1, i);  // h
74     b(j, 0) =  x2(1, i);             // i
75
76     // This ensures better stability
77     // TODO(julien) make a lite version without this 3rd set
78     ++j;
79     L(j, 0) =  x2(1, i) * x1(0, i);  // a
80     L(j, 1) =  x2(1, i) * x1(1, i);  // b
81     L(j, 2) =  x2(1, i);             // c
82     L(j, 3) = -x2(0, i) * x1(0, i);  // d
83     L(j, 4) = -x2(0, i) * x1(1, i);  // e
84     L(j, 5) = -x2(0, i);             // f
85   }
86   // Solve Lx=B
87   Vec h = L.fullPivLu().solve(b);
88   Homography2DNormalizedParameterization<double>::To(h, H);
89   if ((L * h).isApprox(b, expected_precision))  {
90     return true;
91   } else {
92     return false;
93   }
94 }
95
96 /** 2D Homography transformation estimation in the case that points are in 
97  * homogeneous coordinates.
98  *
99  * | 0 -x3  x2|   |a b c|   |y1|   -x3*d+x2*g -x3*e+x2*h -x3*f+x2*1    |y1|   (-x3*d+x2*g)*y1 (-x3*e+x2*h)*y2 (-x3*f+x2*1)*y3   |0|
100  * | x3  0 -x1| * |d e f| * |y2| =  x3*a-x1*g  x3*b-x1*h  x3*c-x1*1  * |y2| =  (x3*a-x1*g)*y1  (x3*b-x1*h)*y2  (x3*c-x1*1)*y3 = |0|
101  * |-x2  x1  0|   |g h 1|   |y3|   -x2*a+x1*d -x2*b+x1*e -x2*c+x1*f    |y3|   (-x2*a+x1*d)*y1 (-x2*b+x1*e)*y2 (-x2*c+x1*f)*y3   |0|
102  * X = |a b c d e f g h|^t
103  */
104 bool Homography2DFromCorrespondencesLinear(const Mat &x1,
105                                            const Mat &x2,
106                                            Mat3 *H,
107                                            double expected_precision) {
108   if (x1.rows() == 2) {
109     return Homography2DFromCorrespondencesLinearEuc(x1, x2, H,
110                                                     expected_precision);
111   }
112   assert(3 == x1.rows());
113   assert(4 <= x1.cols());
114   assert(x1.rows() == x2.rows());
115   assert(x1.cols() == x2.cols());
116
117   const int x = 0;
118   const int y = 1;
119   const int w = 2;
120   int n = x1.cols();
121   MatX8 L = Mat::Zero(n * 3, 8);
122   Mat b = Mat::Zero(n * 3, 1);
123   for (int i = 0; i < n; ++i) {
124     int j = 3 * i;
125     L(j, 0) =  x2(w, i) * x1(x, i);  // a
126     L(j, 1) =  x2(w, i) * x1(y, i);  // b
127     L(j, 2) =  x2(w, i) * x1(w, i);  // c
128     L(j, 6) = -x2(x, i) * x1(x, i);  // g
129     L(j, 7) = -x2(x, i) * x1(y, i);  // h
130     b(j, 0) =  x2(x, i) * x1(w, i);
131
132     ++j;
133     L(j, 3) =  x2(w, i) * x1(x, i);  // d
134     L(j, 4) =  x2(w, i) * x1(y, i);  // e
135     L(j, 5) =  x2(w, i) * x1(w, i);  // f
136     L(j, 6) = -x2(y, i) * x1(x, i);  // g
137     L(j, 7) = -x2(y, i) * x1(y, i);  // h
138     b(j, 0) =  x2(y, i) * x1(w, i);
139
140     // This ensures better stability
141     ++j;
142     L(j, 0) =  x2(y, i) * x1(x, i);  // a
143     L(j, 1) =  x2(y, i) * x1(y, i);  // b
144     L(j, 2) =  x2(y, i) * x1(w, i);  // c
145     L(j, 3) = -x2(x, i) * x1(x, i);  // d
146     L(j, 4) = -x2(x, i) * x1(y, i);  // e
147     L(j, 5) = -x2(x, i) * x1(w, i);  // f
148   }
149   // Solve Lx=B
150   Vec h = L.fullPivLu().solve(b);
151   if ((L * h).isApprox(b, expected_precision))  {
152     Homography2DNormalizedParameterization<double>::To(h, H);
153     return true;
154   } else {
155     return false;
156   }
157 }
158
159 // Default settings for homography estimation which should be suitable
160 // for a wide range of use cases.
161 EstimateHomographyOptions::EstimateHomographyOptions(void) :
162     use_normalization(true),
163     max_num_iterations(50),
164     expected_average_symmetric_distance(1e-16) {
165 }
166
167 namespace {
168 void GetNormalizedPoints(const Mat &original_points,
169                          Mat *normalized_points,
170                          Mat3 *normalization_matrix) {
171   IsotropicPreconditionerFromPoints(original_points, normalization_matrix);
172   ApplyTransformationToPoints(original_points,
173                               *normalization_matrix,
174                               normalized_points);
175 }
176
177 // Cost functor which computes symmetric geometric distance
178 // used for homography matrix refinement.
179 class HomographySymmetricGeometricCostFunctor {
180  public:
181   HomographySymmetricGeometricCostFunctor(const Vec2 &x,
182                                           const Vec2 &y)
183       : x_(x), y_(y) { }
184
185   template<typename T>
186   bool operator()(const T *homography_parameters, T *residuals) const {
187     typedef Eigen::Matrix<T, 3, 3> Mat3;
188     typedef Eigen::Matrix<T, 3, 1> Vec3;
189
190     Mat3 H(homography_parameters);
191
192     Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
193     Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
194
195     Vec3 H_x = H * x;
196     Vec3 Hinv_y = H.inverse() * y;
197
198     H_x /= H_x(2);
199     Hinv_y /= Hinv_y(2);
200
201     // This is a forward error.
202     residuals[0] = H_x(0) - T(y_(0));
203     residuals[1] = H_x(1) - T(y_(1));
204
205     // This is a backward error.
206     residuals[2] = Hinv_y(0) - T(x_(0));
207     residuals[3] = Hinv_y(1) - T(x_(1));
208
209     return true;
210   }
211
212   const Vec2 &x_;
213   const Vec2 &y_;
214 };
215
216 // Termination checking callback used for homography estimation.
217 // It finished the minimization as soon as actual average of
218 // symmetric geometric distance is less or equal to the expected
219 // average value.
220 class TerminationCheckingCallback : public ceres::IterationCallback {
221  public:
222   TerminationCheckingCallback(const Mat &x1, const Mat &x2,
223                               const EstimateHomographyOptions &options,
224                               Mat3 *H)
225       : options_(options), x1_(x1), x2_(x2), H_(H) {}
226
227   virtual ceres::CallbackReturnType operator()(
228       const ceres::IterationSummary& summary) {
229     // If the step wasn't successful, there's nothing to do.
230     if (!summary.step_is_successful) {
231       return ceres::SOLVER_CONTINUE;
232     }
233
234     // Calculate average of symmetric geometric distance.
235     double average_distance = 0.0;
236     for (int i = 0; i < x1_.cols(); i++) {
237       average_distance = SymmetricGeometricDistance(*H_,
238                                                     x1_.col(i),
239                                                     x2_.col(i));
240     }
241     average_distance /= x1_.cols();
242
243     if (average_distance <= options_.expected_average_symmetric_distance) {
244       return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
245     }
246
247     return ceres::SOLVER_CONTINUE;
248   }
249
250  private:
251   const EstimateHomographyOptions &options_;
252   const Mat &x1_;
253   const Mat &x2_;
254   Mat3 *H_;
255 };
256 }  // namespace
257
258 /** 2D Homography transformation estimation in the case that points are in
259  * euclidean coordinates.
260  */
261 bool EstimateHomography2DFromCorrespondences(
262     const Mat &x1,
263     const Mat &x2,
264     const EstimateHomographyOptions &options,
265     Mat3 *H) {
266   // TODO(sergey): Support homogenous coordinates, not just euclidean.
267
268   assert(2 == x1.rows());
269   assert(4 <= x1.cols());
270   assert(x1.rows() == x2.rows());
271   assert(x1.cols() == x2.cols());
272
273   Mat3 T1 = Mat3::Identity(),
274        T2 = Mat3::Identity();
275
276   // Step 1: Algebraic homography estimation.
277   Mat x1_normalized, x2_normalized;
278
279   if (options.use_normalization) {
280     LG << "Estimating homography using normalization.";
281     GetNormalizedPoints(x1, &x1_normalized, &T1);
282     GetNormalizedPoints(x2, &x2_normalized, &T2);
283   } else {
284     x1_normalized = x1;
285     x2_normalized = x2;
286   }
287
288   // Assume algebraic estiation always suceeds,
289   Homography2DFromCorrespondencesLinear(x1_normalized, x2_normalized, H);
290
291   // Denormalize the homography matrix.
292   if (options.use_normalization) {
293     *H = T2.inverse() * (*H) * T1;
294   }
295
296   LG << "Estimated matrix after algebraic estimation:\n" << *H;
297
298   // Step 2: Refine matrix using Ceres minimizer.
299   ceres::Problem problem;
300   for (int i = 0; i < x1.cols(); i++) {
301     HomographySymmetricGeometricCostFunctor
302         *homography_symmetric_geometric_cost_function =
303             new HomographySymmetricGeometricCostFunctor(x1.col(i),
304                                                         x2.col(i));
305
306     problem.AddResidualBlock(
307         new ceres::AutoDiffCostFunction<
308             HomographySymmetricGeometricCostFunctor,
309             4,  // num_residuals
310             9>(homography_symmetric_geometric_cost_function),
311         NULL,
312         H->data());
313   }
314
315   // Configure the solve.
316   ceres::Solver::Options solver_options;
317   solver_options.linear_solver_type = ceres::DENSE_QR;
318   solver_options.max_num_iterations = options.max_num_iterations;
319   solver_options.update_state_every_iteration = true;
320
321   // Terminate if the average symmetric distance is good enough.
322   TerminationCheckingCallback callback(x1, x2, options, H);
323   solver_options.callbacks.push_back(&callback);
324
325   // Run the solve.
326   ceres::Solver::Summary summary;
327   ceres::Solve(solver_options, &problem, &summary);
328
329   VLOG(1) << "Summary:\n" << summary.FullReport();
330
331   LG << "Final refined matrix:\n" << *H;
332
333   return summary.IsSolutionUsable();
334 }
335
336 /**
337  * x2 ~ A * x1
338  * x2^t * Hi * A *x1 = 0
339  * H1 =              H2 =               H3 = 
340  * | 0 0 0 1|     |-x2w|  |0 0 0 0|      |  0 |  | 0 0 1 0|     |-x2z|
341  * | 0 0 0 0|  -> |  0 |  |0 0 1 0|   -> |-x2z|  | 0 0 0 0|  -> |  0 |
342  * | 0 0 0 0|     |  0 |  |0-1 0 0|      | x2y|  |-1 0 0 0|     | x2x|
343  * |-1 0 0 0|     | x2x|  |0 0 0 0|      |  0 |  | 0 0 0 0|     |  0 |
344  * H4 =              H5 =               H6 = 
345  *  |0 0 0 0|     |  0 |  | 0 1 0 0|     |-x2y|   |0 0 0 0|     |  0 |
346  *  |0 0 0 1|  -> |-x2w|  |-1 0 0 0|  -> | x2x|   |0 0 0 0|  -> |  0 |
347  *  |0 0 0 0|     |  0 |  | 0 0 0 0|     |  0 |   |0 0 0 1|     |-x2w|
348  *  |0-1 0 0|     | x2y|  | 0 0 0 0|     |  0 |   |0 0-1 0|     | x2z|
349  *     |a b c d|
350  * A = |e f g h|
351  *     |i j k l|
352  *     |m n o 1|
353  *
354  * x2^t * H1 * A *x1 = (-x2w*a +x2x*m )*x1x + (-x2w*b +x2x*n )*x1y + (-x2w*c +x2x*o )*x1z + (-x2w*d +x2x*1 )*x1w = 0
355  * x2^t * H2 * A *x1 = (-x2z*e +x2y*i )*x1x + (-x2z*f +x2y*j )*x1y + (-x2z*g +x2y*k )*x1z + (-x2z*h +x2y*l )*x1w = 0
356  * x2^t * H3 * A *x1 = (-x2z*a +x2x*i )*x1x + (-x2z*b +x2x*j )*x1y + (-x2z*c +x2x*k )*x1z + (-x2z*d +x2x*l )*x1w = 0
357  * x2^t * H4 * A *x1 = (-x2w*e +x2y*m )*x1x + (-x2w*f +x2y*n )*x1y + (-x2w*g +x2y*o )*x1z + (-x2w*h +x2y*1 )*x1w = 0
358  * x2^t * H5 * A *x1 = (-x2y*a +x2x*e )*x1x + (-x2y*b +x2x*f )*x1y + (-x2y*c +x2x*g )*x1z + (-x2y*d +x2x*h )*x1w = 0
359  * x2^t * H6 * A *x1 = (-x2w*i +x2z*m )*x1x + (-x2w*j +x2z*n )*x1y + (-x2w*k +x2z*o )*x1z + (-x2w*l +x2z*1 )*x1w = 0
360  *
361  * X = |a b c d e f g h i j k l m n o|^t
362 */
363 bool Homography3DFromCorrespondencesLinear(const Mat &x1,
364                                            const Mat &x2,
365                                            Mat4 *H,
366                                            double expected_precision) {
367   assert(4 == x1.rows());
368   assert(5 <= x1.cols());
369   assert(x1.rows() == x2.rows());
370   assert(x1.cols() == x2.cols());
371   const int x = 0;
372   const int y = 1;
373   const int z = 2;
374   const int w = 3;
375   int n = x1.cols();
376   MatX15 L = Mat::Zero(n * 6, 15);
377   Mat b = Mat::Zero(n * 6, 1);
378   for (int i = 0; i < n; ++i) {
379     int j = 6 * i;
380     L(j,  0) = -x2(w, i) * x1(x, i);  // a
381     L(j,  1) = -x2(w, i) * x1(y, i);  // b
382     L(j,  2) = -x2(w, i) * x1(z, i);  // c
383     L(j,  3) = -x2(w, i) * x1(w, i);  // d
384     L(j, 12) =  x2(x, i) * x1(x, i);  // m
385     L(j, 13) =  x2(x, i) * x1(y, i);  // n
386     L(j, 14) =  x2(x, i) * x1(z, i);  // o
387     b(j,  0) = -x2(x, i) * x1(w, i);
388
389     ++j;
390     L(j,  4) = -x2(z, i) * x1(x, i);  // e
391     L(j,  5) = -x2(z, i) * x1(y, i);  // f
392     L(j,  6) = -x2(z, i) * x1(z, i);  // g
393     L(j,  7) = -x2(z, i) * x1(w, i);  // h
394     L(j,  8) =  x2(y, i) * x1(x, i);  // i
395     L(j,  9) =  x2(y, i) * x1(y, i);  // j
396     L(j, 10) =  x2(y, i) * x1(z, i);  // k
397     L(j, 11) =  x2(y, i) * x1(w, i);  // l
398
399     ++j;
400     L(j,  0) = -x2(z, i) * x1(x, i);  // a
401     L(j,  1) = -x2(z, i) * x1(y, i);  // b
402     L(j,  2) = -x2(z, i) * x1(z, i);  // c
403     L(j,  3) = -x2(z, i) * x1(w, i);  // d
404     L(j,  8) =  x2(x, i) * x1(x, i);  // i
405     L(j,  9) =  x2(x, i) * x1(y, i);  // j
406     L(j, 10) =  x2(x, i) * x1(z, i);  // k
407     L(j, 11) =  x2(x, i) * x1(w, i);  // l
408
409     ++j;
410     L(j,  4) = -x2(w, i) * x1(x, i);  // e
411     L(j,  5) = -x2(w, i) * x1(y, i);  // f
412     L(j,  6) = -x2(w, i) * x1(z, i);  // g
413     L(j,  7) = -x2(w, i) * x1(w, i);  // h
414     L(j, 12) =  x2(y, i) * x1(x, i);  // m
415     L(j, 13) =  x2(y, i) * x1(y, i);  // n
416     L(j, 14) =  x2(y, i) * x1(z, i);  // o
417     b(j,  0) = -x2(y, i) * x1(w, i);
418
419     ++j;
420     L(j, 0) = -x2(y, i) * x1(x, i);  // a
421     L(j, 1) = -x2(y, i) * x1(y, i);  // b
422     L(j, 2) = -x2(y, i) * x1(z, i);  // c
423     L(j, 3) = -x2(y, i) * x1(w, i);  // d
424     L(j, 4) =  x2(x, i) * x1(x, i);  // e
425     L(j, 5) =  x2(x, i) * x1(y, i);  // f
426     L(j, 6) =  x2(x, i) * x1(z, i);  // g
427     L(j, 7) =  x2(x, i) * x1(w, i);  // h
428
429     ++j;
430     L(j,  8) = -x2(w, i) * x1(x, i);  // i
431     L(j,  9) = -x2(w, i) * x1(y, i);  // j
432     L(j, 10) = -x2(w, i) * x1(z, i);  // k
433     L(j, 11) = -x2(w, i) * x1(w, i);  // l
434     L(j, 12) =  x2(z, i) * x1(x, i);  // m
435     L(j, 13) =  x2(z, i) * x1(y, i);  // n
436     L(j, 14) =  x2(z, i) * x1(z, i);  // o
437     b(j,  0) = -x2(z, i) * x1(w, i);
438   }
439   // Solve Lx=B
440   Vec h = L.fullPivLu().solve(b);
441   if ((L * h).isApprox(b, expected_precision))  {
442     Homography3DNormalizedParameterization<double>::To(h, H);
443     return true;
444   } else {
445     return false;
446   }
447 }
448
449 double SymmetricGeometricDistance(const Mat3 &H,
450                                   const Vec2 &x1,
451                                   const Vec2 &x2) {
452   Vec3 x(x1(0), x1(1), 1.0);
453   Vec3 y(x2(0), x2(1), 1.0);
454
455   Vec3 H_x = H * x;
456   Vec3 Hinv_y = H.inverse() * y;
457
458   H_x /= H_x(2);
459   Hinv_y /= Hinv_y(2);
460
461   return (H_x.head<2>() - y.head<2>()).squaredNorm() +
462          (Hinv_y.head<2>() - x.head<2>()).squaredNorm();
463 }
464
465 }  // namespace libmv